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Foreword 



Robotics is undergoing a major transformation in scope and dimension. From 
a largely dominant industrial focus, robotics is rapidly expanding into human 
environments and vigorously engaged in its new challenges. Interacting with, 
assisting, serving, and exploring with humans, the emerging robots will in- 
creasingly touch people and their lives. 

Beyond its impact on physical robots, the body of knowledge robotics has 
produced is revealing a much wider range of applications reaching across diverse 
research areas and scientific disciplines, such as: biomechanics, haptics, neu- 
rosciences, virtual simulation, animation, surgery, and sensor networks among 
others. In return, the challenges of the new emerging areas are proving an abun- 
dant source of stimulation and insights for the field of robotics. It is indeed at 
the intersection of disciplines that the most striking advances happen. 

The Springer Tracts in Advanced Robotics (STAR) is devoted to bringing to 
the research community the latest advances in the robotics field on the basis 
of their significance and quality. Through a wide and timely dissemination of 
critical research developments in robotics, our objective with this series is to 
promote more exchanges and collaborations among the researchers in the com- 
munity and contribute to further advancements in this rapidly growing field. 

The monograph, written by John Mullane, Ba-Ngu Vo, Martin Adams and 
Ba-Tuong Vo, is devoted to a field of autonomous robotic systems, which has 
received a great deal of attention by the research community in recent years. 
The contents arc focused on the problem of representing the environment 
and its uncertainty in terms of feature based maps. Random finite sets are 
adopted as the fundamental tool to represent a feature map, and a general 
framework is proposed which eliminates the need for feature management and 
data association, and propagates both feature state and number estimates 
in a joint Bayesian framework. The approaches are tested in a number of 
experiments on both ground based and marine based facilities. 

STAR is proud to welcome yet another volume in the series dedicated to 
the popular area of SLAM! 

Naples, Italy Bruno Siciliano 

April 2011 STAR Editor 



Preface 



This book is intended to demonstrate advances in the field of autonomous 
navigation and to serve as an essential text for academics, researchers, in- 
dustrial scientists and general practitioners involved in robotic mapping, 
sensor modelling and the popular Simultaneous Localisation and Map build- 
ing (SLAM) problem. The book focusses on a critical area of autonomous 
robotics research - the representation of the environment and its uncertainty, 
referred to as the map. Probabilistic maps can be primarily categorised into 
three popular representations: Feature based; occupancy grid and topologi- 
cal. This book focusses on, arguably the most popular and widely used of 
these, the feature based map. Two areas which are essential in designing suc- 
cessful autonomous vehicles are addressed: Feature Based Robotic Mapping 
(FBRM) which assumes a known robot trajectory, and an area which has 
been referred to as the "Holy grail" of autonomous robotics research: Feature 
Based - Simultaneous Localisation and Map Building (FB-SLAM) [1]. Rather 
than the commonly used framework, which stacks feature estimates and their 
measurements into a vector, this book advocates that a more appropriate rep- 
resentation for the map, in both FBRM and FB-SLAM, hereafter referred to 
as simply SLAM, is the Random Finite Set (RFS). 

In both FBRM and SLAM, it is necessary to estimate the location of an 
initially unknown number of features, which represent the environment. In 
current, vector based methods the number of features and their locations, 
are represented by a vector of varying size. Methods are then introduced 
which augment this vector when new features are detected. Data association 
techniques are then necessary to determine which feature elements of this 
vector correspond to which elements of the total current observation, which 
is also typically represented as a vector, containing the measured attributes 
of the currently sensed features. Only then can a Bayesian update of the 
total feature map take place. Hence there is an implicit assumption that im- 
mediately before the update, the number of map states is known. Therefore, 
for a given vehicle trajectory, with error-free data association, linear pro- 
cess and measurement models and white Gaussian noise, optimal estimates 
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of a known number of feature locations are realizable using current, vector 
based, approaches. However, when the intrinsic properties of the map are 
considered (unknown number of insignificantly ordered features), this work 
will demonstrate that Bayes optimality has not yet been established. 

This book therefore proposes generalisations of the classical vector-based 
frameworks for both FBRM and SLAM. These address the concept of Bayes 
optimality for estimation with unknown feature number by formulating both 
FBRM and SLAM as random set estimation problems. The proposed formu- 
lations unify the currently independent concepts of feature management, data 
association and state estimation adopted by previous solutions. In the case 
of FBRM, this occurs through the recursive propagation of a distribution of 
a random finite set of features, when the vehicle's trajectory is known. In the 
case of SLAM, the joint estimation of the vehicle location and the random 
finite set of features is derived. 

In the case of FBRM, the RFS approach yields the propagation of the FB 
map density and leads to optimal map estimates in the presence of unknown 
map size, spurious measurements, feature detection and data association un- 
certainty. The proposed framework further allows for the joint treatment of 
error in feature number and location estimates as it jointly propagates both 
the estimate of the number of features and their corresponding states. In 
the case of SLAM, the vehicle's pose state is also jointly estimated. In both 
cases, under the RFS framework, the need for feature management and data 
association algorithms is eliminated. 

An RFS is simply a finite-set-valued random variable. Similar to random 
vectors, the probability density (if it exists) is a very useful descriptor of an 
RFS, especially in filtering and estimation. However, the space of finite sets 
does not inherit the usual Euclidean notion of integration and density. Hence, 
standard tools for random vectors are not appropriate for random finite sets. 
Mahler's Finite Set Statistics (FISST) provide practical mathematical tools 
and principled approximations for dealing with RFSs [2], [3], based on a 
notion of integration and density that is consistent with point process theory 
[4] . This approach has attracted substantial research interest in the tracking 
community, [5], [6], [3] and this book develops these tools for both FBRM 
and SLAM. 

Finally, in any estimation problem, the notion of estimation error is of 
utmost importance. In all FBRM and SLAM experiments, the measure of 
success should be a clearly defined concept. In much of the vector based, 
SLAM research to date, successful performance is evaluated based on the 
location error of a sub-set of the estimated features. Even if the spatial es- 
timation errors of all of the estimated features were used to estimate the 
performance, the concept is meaningless if the number of features has not 
been estimated correctly, since the goal of SLAM is to estimate both the tra- 
jectory and the map. Therefore, for the sake of gauging the performance of 
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all the FBRM and SLAM results presented in this book, a consistent metric 
for the evaluation of feature map estimation error is presented. This metric 
takes into account the error in the cardinality of the map estimate in terms 
of the number of feature estimates, as well as their spatial locations. 

Singapore John Mullanc 

Perth, Australia Ba-Ngu Vo 

Santiago, Chile Martin Adams 

Perth, Australia Ba-Tuong Vo 

March 2011 
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Chapter 1 
Introduction 



Machines which perceive the world through the use of sensors, make compu- 
tational decisions based on the sensors' outputs and then influence the world 
with actuators, are broadly labelled as "Robots". Due to the imperfect na- 
ture of all real sensors and actuators, the lack of predictability within real 
environments and the necessary approximations to achieve computational 
decisions, robotics is a science which is becoming ever more dependent on 
probabilistic algorithms. Autonomous robot vehicles are examples of such 
machines, which are now being used in areas other than the factory floors, 
and which therefore must operate in unstructured, and possibly previously 
unexplored environments. Their reliance on probabilistic algorithms, which 
can interpret sensory data and make decisions in the presence of uncertainty, 
is increasing. Therefore, mathematical interpretations of the vehicle's envi- 
ronment which consider all the relevant uncertainty are of a fundamental im- 
portance to an autonomous vehicle, and its ability to function reliably within 
that environment. While a universal mathematical model which considers 
the vast complexities of the physical world remains an extremely challenging 
task, stochastic mathematical representations of a robots operating environ- 
ment are widely adopted by the autonomous robotic community. Probability 
densities on the chosen map representation arc often derived and then re- 
cursively propagated in time via the Bayesian framework, using appropriate 
measurement likelihoods. 

Of crucial importance in autonomous navigation is the computational rep- 
resentation of a robot's surroundings and its uncertainty, referred to as the 
map. This book directly addresses this issue, initially in the area of Feature 
Based Robotic Mapping (FBRM) which assumes known robot location, and 
then in the area of Feature Based - Simultaneous Localisation and Map Build- 
ing (FB-SLAM^). The book demonstrates that the commonly used vector 
based methods for FBRM and SLAM suffer many fundamental disadvantages 
when applied to realistic situations. Such situations occur in environments in 
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2 1 Introduction 

which an a-priori unknown number of features are to be estimated and in the 
presence of realistic sensor defects such as missed detections and false alarms. 
This book therefore takes a step back to the basic estimation principles and 
aims of the FBRM and SLAM problems, and shows that a more appropriate 
representation for the map, in both cases, is the Random Finite Set (RFS). 
An RFS is a random variable that take values as finite sets. It is defined by 
a discrete distribution that characterises the number of elements in the set, 
and a family of joint distributions which characterise the distribution of the 
element's values, conditioned on the cardinality [3|. 

To date, three fundamentally different approaches, namely occupancy 
grids (7), FB maps [5] and topological maps [5] have been applied in au- 
tonomous mapping research. Of these, occupancy grids and FB maps have 
emerged as the most popular means of probabilistic, environmental represen- 
tation. Numerous examples of impressive localisation, mapping and naviga- 
tion algorithms which adopt these environment models can be seen both in 
indoor [TU], [U, [H], [13, [H] and outdoor [T5|, [T5j, [T7J, [J, [TSj environ- 
ments. 

The Occupancy Grid approach propagates estimates of landmark existence 
on a grid with a fixed, predetermined number of cells. In environmental repre- 
sentations of this type, the number of map states is therefore predefined, and 
constant and therefore, only the cells' "contents" , which typically correspond 
to the likelihood of an objects existence at that cell's coordinates, need to 
be updated. Hence, the grid, which fully represents the environment, can be 
represented mathematically by either a vector or matrix of predefined, fixed 
dimensions. 

Grid based approaches however suffer from many disadvantages. Standard 
occupancy grid maps do not maintain the dependencies of the occupancy 
probability of each cell. Also, a full map posterior is generally intractable, 
due to the huge combinatorial number of maps which can be represented on 
a grid. Further, the grid size, and its resolution (cell size) must be once and 
for all determined prior to navigation, thus restricting the area which can be 
mathematically represented by the robot. 

FB mapping approaches offer the advantage that the sensor data is com- 
pressed into features (such as point clusters, circles, lines, corners etc.). The 
origins of the feature map can be traced back to the seminal work of Smith 
et. al. |8], in which the environment is assumed to consist of these simpli- 
fied representations of the physical landmarks - the features. The feature 
map representation has since gained wide spread popularity, particularly in 
the outdoor robotics domain due to its ability to estimate large scale maps 
and provide correction information for simultaneous vehicle pose estimation. 
The work of Smith et. al. also first established the " vector of all the spatial 
variables, which we call the system state vector" , i.e. M = [m 1 ,m 2 , ■ ■ ■ ,m m ]. 

FB approaches can be computationally appealing, since few features need 
to be detected and updated per sensor scan, and feature to feature and fea- 
ture to vehicle correlations can be maintained. They fundamentally differ 



1 Introduction 3 

from their grid based counterparts, in that they are required to estimate the 
location of an initially unknown number of objects, represented as features. 
Hence, the number of features and their locations, which fully represent the 
environment, are typically represented by varying the size of a vector. Meth- 
ods are then introduced which augment this vector when new features are 
detected. Data association techniques are then necessary to determine which 
feature elements of this vector correspond to which elements of the total cur- 
rent observation, which is also typically represented as a vector, containing 
the measured attributes of the currently sensed features. Only then can a 
Bayesian update of the feature map take place. This concept, which sum- 
marises the current state of the art in FBRM and SLAM, is shown in Figure 
11.11 It can be seen in the figure that, there is an implicit assumption that 
immediately before the update, the number of map states (j> in Figure ITTTJ) to 
be estimated, is determined by the map management heuristics/filters just 
described. Therefore, with error-free data association and optimal feature 
initialisation routines, optimal estimates of a predefined number of feature lo- 
cations are realizable using current, vector based, linear Gaussian approaches. 
However, when the intrinsic properties of the map are considered (unknown 
number of insignificantly ordered features), Baycs optimality of the true prob- 
lem has not yet been established. As noted in the field of multi-target filtering 
by Mahler ([3], page 571): 

"...having a good estimate of target number is half the battle in multitarget 

tracking. If one has 1,000 measurements but we know that roughly 900 of 

them are false alarms, then the problem of detecting the actual targets has 

been greatly simplified." 

This book advocates that the same principle applies to feature maps in 
robotics. Realistic feature detection algorithms produce false alarms (as well 
as missed detections) , and estimating the true number of features is therefore 
central to the FBRM and SLAM problems. This book therefore addresses the 
concept of Bayes optimality for estimation with unknown feature number, by 
formulating it as a random set estimation problem. The proposed formulation 
unifies the independent filters adopted by previous solutions, and high-lighted 
in Figure 11.11 through the recursive propagation of a distribution of a ran- 
dom finite set of features. This allows for the joint propagation of the FB 
map density and leads to optimal map estimates in the presence of unknown 
map size, spurious measurements, feature detection and data association un- 
certainty. The proposed framework further allows for the joint treatment of 
error in feature number and location estimates as it jointly propagates both 
the estimate of the number of landmarks and their corresponding states, and 
consequently eliminates the need for feature management and data associ- 
ation algorithms. The RFS approach to FBRM and SLAM is depicted in 
Figure O 
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6 1 Introduction 

The main focus of the book applies the concepts of RFS mapping to 
the well known SLAM problem. For a FB map, SLAM requires the joint 
estimation of the vehicle location and the map. As in vector based robotic 
mapping algorithms, vector based SLAM algorithms also require 'feature 
management' as well as data association hypotheses and an estimator to 
generate the joint posterior estimate. Hence, in the final part of this book, 
RFS based recursive filtering algorithms are presented which jointly prop- 
agate both the estimate of the number of landmarks, their corresponding 
states, and the vehicle pose state, again without the need for explicit feature 
management and data association algorithms. 



1.1 Structure of the Book 

The book is divided into three parts. In part I, the question "Why use ran- 
dom finite sets?" is addressed. Chapter[5]summarises fundamental differences 
between RFS and vector based representations of features. The fundamen- 
tal mathematical relationships between map states, observations and vehicle 
pose are examined under both vector and RFS based frameworks. Essen- 
tial components of robot navigation algorithms which are mathematically 
inconcise, when modelled under the vector based framework, are shown to 
be concisely realisable under the RFS framework. The issues of map repre- 
sentation, data association, map management, map error quantification and 
the concise application of Bayes theorem will be summarised in this chapter. 

Chapter [3l introduces mathematical representations which can be used for 
RFSs. As in the case of vector based approaches, full Baycsian estimation 
in the space of features and robot trajectory is intractable for all realistic 
scenarios. This chapter poses the fundamental question, "Given the posterior 
distribution of the map (and trajectory in the case of SLAM), what is the 
Bayes optimal estimate?" Principled estimators are therefore presented here, 
which are capable of representing RFSs in a Bayes optimal manner. The 
probability hypothesis density (PHD) filter is introduced as one of the simplest 
approximations to Bayesian estimation with RFSs. This chapter therefore 
provides the foundations for most of the filtering algorithms for both FBRM 
and SLAM, used throughout the book. 

Although much of the current literature advocates that the "Mapping only 
problem" , addressed in Part II is now a solved problem, Chapter U presents 
new insights to motivate an RFS approach to mapping. By focussing on the 
mapping only problem, an estimation framework which yields Bayes opti- 
mal map estimates in the general case of unknown feature number, spurious 
sensor measurements, feature detection and data association uncertainty is 
developed. Further, Chapter 0] examines in more detail, the concept of FB 
map estimation error, for the general case of an unknown number of features. 
This leads to the application of an error metric which is defined on the state 
space of all possible feature maps. 
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Part III addresses the full SLAM problem. Chapter [5] offers a "brute force" 
solution to the SLAM problem using RFSs, as it models the joint vehicle tra- 
jectory and map as a singular RFS, and recursively propagates its first order 
moment. A first order approximation of the RFS state recursion is imple- 
mented which utilises the PHD filter. Under Gaussian noise assumptions, an 
extended Kalman Gaussian mixture implementation is used to implement 
the PHD-SLAM filter. The filter jointly estimates the vehicle pose, feature 
number in the map and their corresponding locations. Assuming a mildly 
non-linear Gaussian system, an extendcd-Kalman Gaussian Mixture imple- 
mentation of the recursion is then tested for SLAM. Simulations demonstrate 
SLAM in the presence of data with a high rate of spurious measurements, 
and comparisons with vector based SLAM are shown. 

In Chapter|B]a Rao-Blackwellised (RB) implementation of the PHD-SLAM 
filter is proposed based on the Gaussian mixture PHD filter for the map and a 
particle filter for the vehicle trajectory. This applies a trajectory conditioned, 
PHD mapping recursion to the SLAM problem, in a similar manner to the 
well known FastSLAM algorithm [T7]. In this sense, each particle, represent- 
ing a single, hypothesised vehicle trajectory, maintains its own, conditionally 
independent PHD map estimate. It will be shown in this chapter that the 
EKF approximation used to represent each trajectory-conditioned map in 
FastSLAM is not valid under the RFS framework. Therefore the likelihood 
of the measurement set, conditioned on the robot's trajectory, but not the 
map, is derived in closed form. This allows the weight for each particle (rep- 
resenting a possible robot trajectory) to be calculated correctly. This chapter 
further demonstrates that, under the PHD map representation, the unique 
ability of map averaging can be achieved, in a principled manner. Simulated 
as well as real experimental results are shown. Also, comparisons with clas- 
sical vector based SLAM algorithms and their various feature management 
routines, demonstrate the merits of the proposed approach, particularly in 
situations of high clutter and data association ambiguity. 

Finally, Chapter [7| demonstrates that the RFS-FBRM and SLAM frame- 
works allow other approximations and implementations, besides those of the 
basic PHD filter, to be made. PHD-SLAM estimates the PHD of the map, 
encompassing the expected number of features, and the vehicle trajectory. 
Chapter [7] will show that the estimated PHD can be appended with the 
distribution of the number of features, as opposed to just its mean value. 
In this chapter, the Cardinalised-PHD (CPHD) filter will be introduced in 
which both the PHD and the feature cardinality distribution are estimated in 
predictor - corrector form. The map can then be readily extracted from the 
posterior cardinality distribution. Further, a Multi-Bernoulli representation 
of an RFS will be introduced, which allows each map feature to have its own 
probability of existence, yielding a valid density function which jointly cap- 
tures its existence as well as spatial uncertainty. Known as the Cardinaliscd 
Multi-target Multi-Bernoulli (CMeMBer) SLAM filter, Chapter [7| shows how 
the existence probability and spatial density of each feature, within a robot 
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trajectory's map, can be propagated forward in time as measurements ar- 
rive. Under the suggested improved RFS approximations, the accuracy of 
the map and trajectory estimates would be expected to out perform those of 
the standard PHD-SLAM estimators of Chapters [5] and |6l 



Part I 
Random Finite Sets 



Chapter 2 

Why Random Finite Sets? 



2.1 Introduction 

We begin the justification for the use of RFSs by re-evaluating the basic issues 
of feature representation, and considering the fundamental mathematical re- 
lationship between environmental feature representations, and robot motion. 
We further the justification for the use of RFSs in FBRM and SLAM by con- 
sidering an issue of fundamental mathematical importance in any estimation 
problem - estimation error. 



2.2 Environmental Representation: Fundamentals 

2.2.1 FBRM and SLAM New Concepts 

Consider a simplistic, hypothetical scenario in which a mobile robot traverses 
three different trajectories, amongst static objects, as shown in Figure ETT1 If 
the trajectory taken by the robot were X 1 (red), then it would seem logical 
that an on board sensor, with a limited range capability, may sense feature 
mi followed by TO2 followed by 7713 etc. Hence after completing trajectory X , 
if a vector M is used to represent the map, then the estimated map could be 

M = [mi m-2 mz "14 777,5 m?, 777,7] (2-1) 

Alternatively, had the robot pursued trajectory X 2 (blue) instead, the order 
in which the features would be sensed would likely be very different, and the 
resulting estimated map could be 

M = \m,& ?TJ2 TO3 rrii TO5 m-j me] (2-2) 



J. Mullane ct al.: Random Finite Sets for Robot Mapping & SLAM, STAR 72, pp. 11 -25 
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m,« 




Fig. 2.1 A hypothetical scenario in which a mobile robot executes three different 
trajectories X 1 , X 2 , X 3 , amidst static objects (features) mi to m-j. 



and had the robot pursued trajectory X 3 (black), the following estimated 
map vector could result 

M = [ttiq 777,7 m 5 m 4 m 3 m 2 iTii] ■ (2-3) 

Since the order of the elements within a vector is of importance (a change 
in the order yields a different vector), three different map vectors result. 
However, since the map features themselves were assumed static, it seems odd 
that this estimated vector is actually dependent on the vehicle's trajectory. 
In a strict mathematical sense, the order of the features within the map 
estimate should not be significant, as any permutation of the vectors results 
in a valid representation of the map. By definition, the representation which 
captures all permutations of the elements within the vector, and therefore 
the features in the map, is a finite set M, whose estimate M. would be as 
shown in representation [ 



M = [mi fni "73 "14 "75 ?776 m 7] T 

M = [7774 777,2 "73 IWl "75 ffVj 7776] T 

M = [ttiq m-j 7775 777,4 "73 7772 T77i] T 



A4 = {mi 777,2 "7,3 7774 7775 777g 7777} (2-4) 



2.2 Environmental Representation: Fundamentals 



13 



Note that for notational purposes, we denote vector representations in italics 
(e.g. for the map M) and set representations in mathcal format (e.g. for the 
map M). 



2.2.2 Eliminating the Data Association Problem 

For most sensors/sensor models considered in SLAM, the order in which 
sensor readings are recorded at each sampling instance, simply depends on the 
direction in which the vehicle/sensor is oriented, and has no significance on 
the state of the map, which typically evolves in a globally defined coordinate 
system, independent of the vehicle's pose. This is illustrated in Figure [2751 in 
which a measurement to state assignment problem is evident. It can be seen 
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\ 



Fig. 2.2 The order in which observations (features) z\ to 27 are detected/extracted 
from the sensor data is usually different from the order of the currently estimated 
features vn\ to 7717 in the state vector. 



in the figure, that even for an ideal sensor, which is always able to detect 
all of the features, all of the tinrc, under the vector based representation, a 
re-ordering of the observed feature vector Z is necessary. This is because, in 
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general, observed feature z\ will not correspond to the current estimate m\ 
etc. (Figure 12. 2\ and the correct feature associations must be determined - 
i.e.: 

Z = [z\ z 2 z 3 z 4 z 5 z 6 z 7 ] T 

/ i \ ??? Feature Association (2.5) 

M = [mi m 2 m 3 1TI4 UI5 ttiq m 7 ] T 

It can be seen in Figure 11.11 that this data association step is necessary, 
before any vector based, Bayesian update can take place. Hence, current 
vector based formulations of the FBRM and SLAM problems require this 
feature association problem to be solved prior to the Bayesian (EKF, UKF 
etc) update. This is because, feature estimates and measurements are rigidly 
ordered in their respective finite vector valued, map states. 

The proposed RFS approach on the other hand, represents both features 
and measurements as finite valued sets M. and Z respectively, which assume 
no distinct ordering of the features, as shown in representations l2~4l and l2~6l 

Z = \z\ z 2 z 3 z 4 z 5 z 6 z 7 ] T 

Z = [z 4 z 2 z 3 z\ z 5 z 7 z 6 ] T 

Z = [z 6 z 7 z 5 z 4 z 3 z 2 z{\ T 



Z = {zi z 2 z 3 z 4 z 5 z 6 z 7 } (2.6) 



Since the finite set representations 12.41 and 12.61 naturally encapsulate all pos- 
sible permutations of the feature map and measurement, feature associa- 
tion assignment does not have to be addressed. This will be demonstrated 
throughout the book. 



2.2.3 Eliminating the Map Management Problem 

For the more realistic case of non-ideal sensors/feature extraction algorithms, 
the number of measurements, 3^, at any given time is not fixed due to de- 
tection uncertainty, spurious measurements and unknown number of true 
features. As the robot moves through its environment, more and more fea- 
tures are detected, as they enter the field of view (FoV) of its sensors. Hence 
the map size grows monotonically as shown in Figure [2~^1 In the figure it can 
be seen that 5 features have been detected, although there are seven features 
in the environment shown. Objects ms, m^ and m 7 lie out of range of the 
sensor, in the robot's current position. Due to sensor and/or feature detection 
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Fig. 2.3 Feature detection with a more realistic sensor. As the robot moves, new 
features will enter the FoV of the sensor(s). In general, some features may be un- 
detected (missed detections), and some falsely detected features (false alarms) may 
be declared, due to less than ideal sensor/feature detection algorithm performance. 



algorithm imperfections, two false alarms z% and Z4 have occurred. These can 
originate from clutter, sensor noise or incorrect feature detection algorithm 
performance. Notice also, that although object m-i lies within the FoV of the 
sensor, it has not been detected, and constitutes a missed detection. 

Suppose that features mi, mi and 7713 already exist at time k — 1 in a 
vector based map representation, and that feature TO4 now falls into the 
robot's sensor(s) FoV. Feature m^ is to now be initialised and included in 
the state estimate at time k. From a strict mathematical point of view, it is 
unclear, using a vector based framework, how this should be carried out, as 
shown in equation [2771 

M fc _i = [mi m 2 m 3 ] T 

M k = [mi m 2 m 3 ] T " + "[TO 4 ] (2.7) 

where = is used here to mean "how do we assign M^?" M/~-i represents 
the vector based state at time k — 1 and M^ the corresponding state at 
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time k. A clear mathematical operation for combining vectors of different 
dimensions is not denned. To date, many FBRM and SLAM techniques use 
vector augmentation methods. However if the map is defined as a set, then a 
set based map transition function can be mathematically defined as 

Mk-i = {mi m 2 TO3} 

M k = {m 1 m 2 m 3 }U{rn4} (2.8) 

Another fundamental component of any FBRM or SLAM framework is a 
necessity to relate observations to the estimated state. As can be seen in 
equations 12.91 the relationship between observations and the estimated state 
is not clearly defined under a vector based framework. 

Zk = h([mi mi m-i TO4], Xk) + noise 
i.e.: \z\ z-i z% 24 z$] = h{[m\ m 2 1TI3 1114], Xk) + noise (2.9) 

where Zk represents the observation vector at time k, and here the observa- 
tion example of Figure l2~3l is used. Xk represents the vehicles pose at time k 
and h() is the (typically non-linear) function relating map feature locations 
and the vehicle pose, to the observations. Equation 12.91 highlights the prob- 
lem of relating, for example, five observations to just four feature locations, 
and the robot's pose. The extra observed features are clearly the result of 
clutter in this case, and one feature has been missed (undetected). How such 
"clutter" observations, and missed detections can be incorporated into the 
vector based measurement equation is undefined. Clearly, assuming that sin- 
gle features give rise to at most single observations, there is an inconsistency, 
due to the mismatch in the map state and observation vectors' dimensions. 
In the case of vectors, map management heuristics are typically used to first 
remove one of the observed features so that the equation can be "forced to 
work" . 

If set based measurements and state map estimates are used, a strict math- 
ematical relationship is possible as shown in equation 12. 101 

Z k = U Vk(m,Xk)UCk(X k ) (2.10) 

meMk 

where T>k(m,Xk) is the RFS of measurements generated by a feature at 
to, and dependent on Xk and Ck(Xk) is the RFS of the spurious measure- 
ments at time k, which may depend on the vehicle pose Xk- Therefore 
Zk = {z\, z\, . . . , z^} consists of a random number, 3^, of measurements, 
whose order of appearance has no physical significance with respect to the 
estimated map of features. 

As a result of the data association methods and map management rules 
which are necessary when the vector based representation is used for FBRM 
and SLAM, it is clear that the uncertainty in the number of features is not 
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modelled. Typically, post-processing (outside of the Bayesian estimation com- 
ponent) filters are required to estimate the feature number [TJ]. If an RFS 
approach is used however, the uncertainty in both the feature state values 
(typically locations) and number can be modelled in a consistent, joint math- 
ematical manner. 



2.3 FBRM and SLAM Error Quantification 

Fundamental to any state estimation problem is the concept of estimation 
error. While the concept of error quantification is well established in the oc- 
cupancy grid literature [T§1E3, in the feature-based literature the topic is 
less rigorously addressed. Current error evaluations of feature-based frame- 
works typically analyse the consistency of a subset of the feature location 
estimates [T] , [T5] . While this may illustrate the consistent spatial state esti- 
mates of the selected features, it gives no indication as to the quality of the 
estimate of the joint multi-feature map state. Qualitative analysis, in which 
estimated map features and robot location are superimposed on satellite im- 
ages [17j , is also not mathematically consistent and overlooks the underlying 
estimation problems of the feature map, namely that of the error in the es- 
timated number and location of features in the map. 

Whilst the majority of autonomous navigation work focuses on the lo- 
calisation accuracy that can be achieved, the accuracy of the resulting map 
estimate is of equal importance. A precise measurement of the robots sur- 
roundings is essential to any task or behaviour the robot may be required to 
perform. A broad range of exteroceptive sensors are generally deployed on au- 
tonomous vehicles to acquire information about the surrounding area. Many 
sensors, such as laser range finders, sonars and some types of radar, measure 
the relative range and bearing from the vehicle to environmental landmarks 
and are used to update the time predicted map state. Such measurements 
are however subject to uncertainty such as range and bearing measurement 
noise, detection uncertainty, spurious measurements and data association un- 
certainty [20]. p]. 

This section demonstrates that, in the context of jointly evaluating the 
error in the estimated number of features and their locations, and their true 
values, the collection of features, should be represented by a finite set. The 
rationale behind this representation traces back to a fundamental consider- 
ation in estimation theory - estimation error. Without a meaningful notion 
of estimation error, estimation has very little meaning. Despite the fact that 
mapping error is equally as important as localisation error, its formal treat- 
ment has been largely neglected. 

To illustrate this point, recall that in existing SLAM formulations the map 
is constructed by stacking features into a vector, and consider the simplistic 
scenarios depicted in figure YTM Figure ETlk depicts a scenario in which there 
are two true features at coordinates (0,0) and (1,1). The true map, M, is 



IK 



2 Why Random Finite Sets? 







■ 


■ 




O True Features 
• Estimated Features 




©J 







© / 


1| 


M = 



1 


A 0:k 


1 . 






1 




o] 



Meters 

(a) 







■ 


■ 




O True Features 

• Estimated Features 




©/ 


M = 




1 


Y r m = 

A 0:k 


'11 
.if 




1 





2 3 



Meters 

(b) 



Fig. 2.4 A hypothetical scenario showing a fundamental inconsistency with vector 
representations of feature maps. If M is the true map, how should the error be 
assigned when the number of features in the map estimate, M, is incorrect? 



then represented by the vector M — [0 1 1} T . If features are stacked into 
a vector in order of appearance then, given a vehicle trajectory X 0: k (e.g. 
as shown in the figure) and perfect measurements, the estimated map may 
be given by the vector M= [1 1 0] T . Despite a seemingly perfect estimate 
of the map, the Euclidean error of the estimated map, \\M — M\\, is 2. This 
inconsistency arises because the ordering of the features in the representation 
of the map should not be relevant. A vector representation however, imposes 
a mathematically strict arrangement of features in the estimated map based 
on the order in which they were detected [H] , [T] . Intuitively, the elements of 
M could be permuted to obtain a zero error, however, the representation of 
all possible permutations of the elements of a vector is, by definition, a set. 
Hence, such a permuting operation implies that the resulting error distance 
is no longer a distance for vectors but a distance for sets, and thus this 
book derives a set based approach to SLAM. Another problem is depicted in 
figure l2~4b . in which there are again two features at (0,0) and (1,1), but due 
to a missed detection (for instance), the estimated map comprises only one 
feature at (1,1). In such a situation, it is difficult to define a mathematically 
consistent error metric (Euclidean error, Mean Squared Error) between the 
vectors M and M since they contain a different number of elements. It is 
evident from these examples that stacking individual features into a single 
vector does not lead to a natural notion of mapping error, in general. 

A finite set representation of the map, Jv[ k = {m\, . . .,m™ k }, where 
rn , . . . , m mk are the rrifc features present at time k, admits a mathemati- 
cally consistent notion of estimation error since the 'distance', or error be- 
tween sets, is a well understood concept. Examples of such 'distance' metrics 
include the Hausdorff, Optimal Mass Transfer (OMAT) [22j and Optimal 
Sub-pattern Assignment (OSPA) [23] distances. 
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To the authors' knowledge, despite widespread quantification of localisa- 
tion estimation error, the absolute difference between all estimated and actual 
features in the map is rarely jointly considered.} As an example, Figure [231 
shows a hypothetical posterior map estimate returned by two separate fea- 
ture mapping filters. If the true feature map, M = {m 1 , . . . , m mfc } (shown as 
green circles) and the estimated map M = {m 1 , . . . ,fh mk } (shown as black 
crosses) , where m^ is the total number of features in the map and rrifc is the 
estimated number of features in the map, which map estimate is closer to 
Ml. While visual perception may indicate that the left-hand map estimate 
is superior, an accepted metric to answer this fundamental question is lack- 
ing in the mobile robotics community. Suitable error metrics to address this 
problem, will be the subject of Chapter 5J 





-15 -10 



15 -15 -10 -5 



Fig. 2.5 Hypothetical posterior estimates from a feature mapping filter, Mi e f t and 
M r i g ht, with true feature locations (green circles) and estimated feature locations 
(black crosses) shown. 



2.4 Bayesian FBRM and SLAM with Vectors and Sets 



This section outlines the Bayesian recursion which is central to the ma- 
jority of FBRM and SLAM stochastic mapping algorithms. Let M denote 
a generic mathematical representation of the environment to be mapped, 
Zk = {z\, ..., zl k } denote the collection of fa sensor measurements at time k 
and Xk be the vehicle pose, at time k. In the case of FBRM, the aim is to 
then propagate the posterior density of the map from the measurement and 
pose history, Z 0: k — [Zi,...,Zk] and X 0: k = [Xi,...,Xk] respectively. Maxi- 
mum a posteriori (MAP) or expected a posteriori (EAP) estimates may then 
be extracted from the posterior density at each time k. 



1 Approaches examining the consistency of a subset of feature estimates are com- 
mon however [T1ll5|. 
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Assuming that such a density exists, from an optimal Bayesian perspective 
the posterior Probability Density Function (PDF) 

Pk\k(M\Z 0:k ,X 0:k ) 

captures all the relevant statistical information about the map, up to and in- 
cluding time k. The posterior PDF of the map can in principle be propagated 
in time via the well-known Bayes recursion, 

p k]k (M\Z .. k ,X 0:k ) ex g k (Z k \M,X k )p k \ k _ 1 (M\Z .. k . 1 ,X k . 1 ). (2.11) 

For clarity of exposition, this static mapping only problem is adhered to in 
the first part of this book. This formulation can however be easily extended 
to the SLAM problem in which the full posterior p k \ k (Xo :k , M k \Zg- k ) can be 
propagated in time. The formulation can be further extended to incorporate 
dynamic maps and multiple vehicle SLAM, which will be the subject of the 
final chapter of this book. 

A mathematical representation of the map, M, is required before the like- 
lihood, g k (Z\M, X k ), and prior, p k \ k -\{M |,Zo:fc-ii X k _i), can be well defined. 
Bayesian based estimation of both occupancy grid (OG), vector FB and RFS 
FB map representations are now addressed. The following sections highlight 
the advantages of RFS over vector based formulations, in terms of Bayes 
optimality. 



2-4- 1 Bayesian Estimation with Occupancy Grids 

Since its inception by Moravec and Elfes [7], the occupancy grid map, denoted 
M = [m 1 ,m 2 , ■ ■ ■ ,m m ], has been widely accepted as a viable mathematical 
representation of a given environment. In the context of an Occupancy Grid, 
m, represents a fixed number of spatial cells, usually distributed in the form 
of a lattice, which are obtained via an a priori tessellation of the spatial state 
space. Each grid cell is then denoted 'Occupied', if a landmarlo exists in the 
cell, and 'Empty', if the cell is empty of landmarks. The recursion of equation 
12.111 then propagates the posterior density on the occupancy grid, typically 
by invoking a grid cell independence assumption, 

i— m 

Pk\k(M\Z . k ,X 0:k ) = Y]_Pk\k(m l \Z -.k, X 0:k ) 

with p k \ k (m t \Za [k , Xo :k ) denoting the probability, a, of a landmark existing 
in cell m l . The occupancy grid environment representation is attractive due 



2 Note in this work, a 'landmark' refers to any physical object in the environment. 
A 'feature' then refers to a simplified representation of a landmark. 
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to its ability to model arbitrary landmarks as the cell number tends to infin- 
ity. An important, and rarely examined aspect of the grid approach however, 
is that the number of grid cells, m, is inherently known a priori. This has 
a fundamental impact on the optimality of the Bayesian recursion since it 
means that only the occupancy of each cell needs to be estimated and not 
the number of grid cells. Thus a vector-valued map state can be used to 
represent the grid cells since, in this case, it is not necessary to encapsulate 
uncertainty in the number of states. Given the existence estimation state 
space of the representation, stochastic detector dependent measurement like- 
lihoods are also required |16j . Much of the grid based mapping literature 
distributes occupancy uncertainty in the spatial space to model the uncer- 
tainty of the sensing and map estimation process |23], [5D]. However, while 
this environmental representation deals with detection and spurious measure- 
ments to propagate the landmark existence estimate, such a representation 
in its mathematical structure does not inherently encapsulate and propagate 
the spatial uncertainty of sensor measurements |16) . This will be explained 
further in Section 12.51 A true spatial state space is explicitly considered in 
the feature map representation described next. 



2-4-2 Bayesian Estimation with a Vector Feature Map 

While defining a vector-valued feature map representation may appear to be 
a trivial case of terminology, in fact it has already been demonstrated that 
it has numerous mathematical consequences [3], namely an inherent rigid 
ordering of variables and a fixed length. The feature map approach has long 
been recognised as a "a state estimation problem involving a variable number 
of dimensions (features)" [25] , however a vector representation for a feature 
map can only represent a fixed number of features. That is, the posterior 
vector feature map density, 

Pklk (M=[m 1 ,m 2 ,--- ,m^}\Zo: k ,X : k ) 

represents the spatial density of m k features only, and does not encapsu- 
late uncertainty in feature number. This limitation of vector representations 
is not new to robotics researchers and the sub-optimal map management 
methods mentioned in Section 12.21 and shown in Figure 11.11 are subsequently 
adopted to adjust the estimate of rrifc through 'augmenting' and 'pruning' fil- 
tering/heuristic based operations [IT], [T]. More advanced methods, which al- 
low reversible data association across a finite window of time frames have also 
been considered [IB], [IS]- Furthermore, the order of the features 1, . . . , rhfc in 
the vector is fixed, coupled with a vector- valued measurement equation (also 
of rigid order), which leads to the need for costly data association algorithms 
to decide the measurement-feature assignment. This can be seen in the case 
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of SLAM, as applying Bayes theorem (equation l2.1ip to a vector valued map 
involves the following steps: 

• Predicted time update, based on the previous vehicle pose and previous 
inputs to the robot (typically speed, steering commands): 

Pfc|fc— 1( Xo:kiMk\Zo : k-l,Uo:k-liXo) = 

/ x (Xo..fc,M fc |Xo :fc _i,M fc _i,£/fc_i) x 

Pk-l\k-l(Xo-.k-l, Mk-l\Zo:k-l, Uo:k-2,X )dXk-l (2.12) 

• Acquire the measurement vector Z k . 

• Carry out feature associations before the application of Bayes theorem. 

• Perform the measurement update: 

Pk\k{X 0:fc, Mk\Zo:k,Uo : k-l, Xq) = (2-13) 

gk(Zk\Mk, Xk)Pk\k-l(Xo:k> Mk\Zo;k-l, Uo:h-l,Xo) 

J J g k {Z k \M k , X k ) Pk \ k _ 1 (Xo :k ,Mk\Z 0:k -i, U .. k - 1 ,Xo)dX k dM k 

• Perform independent map management. 

It is important to note that when both the measurement likclihood_|g; c (.Zfc|Mt;, 
X k ) and the predicted SLAM state Pk\k-i(Xo-.k, M k \Z -. k -i, Uo-.k-i, Xq), in 
the numerator of eauation l2.13l are represented by random vectors, they must 
be of compatible dimensions before the Bayes update can be carried out. This 
is why the independent data association step is necessary. It is also of im- 
portance to note that the SLAM state and feature number are not jointly 
propagated or estimated. 

The next section introduces the finite set representation for a feature map, 
which yields the joint encapsulation of the feature number and spatial uncer- 
tainty as well as their optimal joint estimation. 

2-4-3 Bayesian Estimation with a Finite Set Feature 
Map 

Inconsistencies in the classical vector feature map representation can be 
demonstrated through a simple question: How is a map with no features 
represented by a vector? A set can represent such a case through the null 
set. Furthermore, due to the unknown number of features in a map and the 



3 Note the notational change for the measurement likelihood. Throughout this 
book, the pt notation is used only on the densities from which state estimates 
are to be extracted via a suitable Bayes optimal estimator. While not commonly 
used, we believe that denoting the measurement likelihood by gk, adds clarity 
and improves readability. 



2.4 Bayesian FBRM and SLAM with Vectors and Sets 23 

physical insignificance of their order, the feature map can be naturally rep- 
resented as a finite set, M. = {m 1 , m 2 , • • • , m m }. A random finite set (RFS) 
then encapsulates the uncertainty in the finite set, i.e. uncertainty in feature 
number and their spatial states. Thus, an RFS feature map can be com- 
pletely specified by a discrete distribution that models the uncertainty in the 
number of features, and continuous joint densities that model their spatial 
uncertainty, conditioned on a given number estimate. In a similar vein to the 
previous vector feature map (for FBRM), an RFS can be described by its 
PDF 

p k \ k (M = {m 1 ,m 2 ,--- ,m™"}\Z Q:k ,X Q:k ) 

and propagated through a Bayesian recursion as follows: 

• Predicted time update, based on the previous vehicle pose and previous 
inputs to the robot: 

Pk\k-l( Xo : k,M.k\Zo:k-l,Uo;k-l,Xo) — 

fx(X ..k,Mk\Xo:k-i,M k -i,U k -i) x 

Pk-l\k-l(Xo:k-l,Mk-l\Zo:k-l,Uo :k -2,X )dX k -i (2.14) 

• Acquire the measurement set Z k . 

• Perform the measurement update: 

Pk\k{ Xo;k,Mk\Zo:k,Uo : k~i,X ) = (2-15) 

gk(Zk\Mk,X k )p k i k _ 1 (X 0:k ,Mk\Z 0:k - 1 ,U 0:k - 1 ,Xo) 
J J gk(Z k \M k ,X k )p k \ k _ 1 (X 0:k ,M k \Z 0:k _ 1 ,U ..k-i,Xo)dX k 6Mk 

where 5 implies set integration. 

Contrary to the vector based implementation of Bayes theorem in equation 
12.131 it is important to note that the measurement likelihood g k (Z k \A4 k , X k ) 
and predicted SLAM state p k \ k _i(X 0:k ,M k \Z 0:k -i,Uo: k -i,X ) in the nu- 
merator of equation 12.151 are finite set statistics (FISST) representing the 
RFS, which do not have to be of compatible dimensions. 

Integration over the map in the denominator of equation 12.151 requires 
integration over all possible feature maps (all possible locations and numbers 
of features). By adopting an RFS map representation, integrating over the 
map becomes a set integral. This feature map recursion therefore encapsulates 
the inherent feature number uncertainty of the map, introduced by detection 
uncertainty, spurious measurements and vehicle manoeuvres, as well as the 
feature location uncertainty introduced by measurement noise. Features are 
not rigidly placed in a map vector, nor are measurements simply a direct 
function of the map state, due to the explicit modelling of clutter. Therefore, 
contrary to previous vector represented approaches, no explicit measurement- 
feature assignment (the data association problem) is required. 
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Hence, by adopting an RFS representation of the mapped and observed 
features, Bayes theorem can be applied to jointly estimate the feature state, 
number and vehicle pose for SLAM. 

2.5 Further Attributes of the RFS Representation 

To date, a map representation which unifies the existence filtering state- 
space of the occupancy map representation and the spatial state-space of 
the feature map representation remains elusive. While previous researchers 
generally adopt independent filters to propagate the spatial and existence 
posteriors of a vector feature map, such an approach leads to some theoretical 
inconsistencies. For instance, consider the posterior density for a single feature 
map, p k i k (M=[m]\Zo :k ). In order for the Bayesian recursion of equation l2.11l 
to be valid, the density must be a PDF, i.e. f p k \ k (M\Z 0: k)dM = 1. This 
however implicitly implies that the feature definitely exists somewhere in the 
map. By using a separate existence filter to obtain an existence probability of 
a, the implication is that J p k \ k (M\Z 0]k )dM = a, which subsequently violates 
a fundamental property of a PDF Va^l. For such a case, it is evident that 
a vector-valued feature map representation cannot jointly incorporate feature 
existence and location uncertainty. 

An RFS framework can readily overcome these issues. For instance, an 
analogous joint recursion can be obtained by adopting a Poisson RFS to 
represent the feature map. This approach does not maintain an existence 
estimate on each feature, but instead propagates a density which represents 
the mean number of features in the map as well as their spatial densities. 
An alternative RFS map model is a multi-Bernoulli RFS, as will be shown 
in Chapter 151 (eq uation 13 . 1 [) . which can jointly encapsulate the positional and 
existence uncertainty of each individual feature under a valid PDF, which can 
be subsequently propagated and estimated via the so called McMBer Filter. 



2.6 Summary 

This chapter has provided several motivations for the theoretical represen- 
tation of feature based maps to take the form of RFSs as opposed to the 
classically used random vectors. Indeed it has been demonstrated that a vec- 
tor representation of the map introduces many algorithmic/mathematical 
consequences, in the forms of the ordering of features within the estimated 
map and observation vectors; the data association problem; the map man- 
agement problem; feature map error quantification and the problems of vec- 
tor dimensionality differences within a vector based, Bayes recursion. It was 
demonstrated that these mathematical consequences result in algorithmic 
routines which typically augment or truncate vectors outside of the Bayesian 
FBRM/SLAM recursions, resulting in Bayes optimality only being possible 
on a predetermined subset of the detected features. 
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The RFS representation has been conceptually introduced as a means in 
which the Bayes optimal estimation of both feature number and spatial state, 
is achievable without the need for such pre-Bayesian augmenting/truncating 
methods. Indeed, it was highlighted that no data association is necessary at 
all, under the RFS framework. This naturally leads us to the scope of Chapter 
[21 in which mathematically tractable, RFS based approximations are derived, 
for Bayes optimal FBRM and SLAM. 



Chapter 3 

Estimation with Random Finite Sets 



3.1 Introduction 

The previous chapter provided the motivation to adopt an RFS representa- 
tion for the map in both FBRM and SLAM problems. The main advantage 
of the RFS formulation is that the dimensions of the measurement likeli- 
hood and the predicted FBRM or SLAM state do not have to be compatible 
in the application of Bayes theorem, for optimal state estimation. The im- 
plementation of Bayes theorem with RFSs (equation I2.15[) is therefore the 
subject of this chapter. It should be noted that in any realistic implemen- 
tation of the vector based Bayes filter, the recursion of equation 12.131 is, in 
general, intractable. Hence, the well known extended Kalman filter (EKFs), 
unscentcd Kalman filter (UKFs) and higher order filters are used to approx- 
imate multi-feature, vector based densities. Unfortunately, the general RFS 
recursion in equation 12.151 is also mathematically intractable, since multiple 
integrals on the space of features are required. This chapter therefore intro- 
duces principled approximations which propagate approximations of the full 
multi-feature posterior density, such as the expectation of the map. Tech- 
niques borrowed from recent research in point process theory known as the 
probability hypothesis density (PHD) filter, cardinalised probability hypothe- 
sis density (C-PHD) filter, and the multi-target, multi-Bernoulli (McMBcr) 
filter, all offer principled approximations to RFS densities. A discussion on 
Bayesian RFS estimators will be presented, with special attention given to 
one of the simplest of these, the PHD filter. In the remaining chapters, vari- 
ants of this filter will be explained and implemented to execute both FBRM 
and SLAM with simulated and real data sets. 

The notion of Bayes optimality is equally as important as the Bayesian 
recursion of equation 12.151 itself. The following section therefore discusses 
optimal feature map estimation in the case of RFS based FBRM and 
SLAM, and once again, for clarity, makes comparisons with vector based 
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estimators. Issues with standard estimators are demonstrated, and optimal 
solutions presented. 



3.2 Classical State Estimators 

In this section, we pose a fundamental question: "Given the posterior dis- 
tribution of the map/SLAM state, what is the Baycs "optimal" estimate?" 
While an RFS map representation can jointly encapsulate feature number and 
location uncertainty, the problem of extracting the optimal estimate from the 
posterior density (in the case of RFS SLAM), p k \ k (X -.k, -Mk\Zo-.k, Uo-.k-i,X ) 
of equation 12.151 is not straight forward. This section therefore outlines cer- 
tain technical inconsistencies of traditional estimators, leading to summaries 
of principled approaches in Section [3731 (for more details see [3J, [27]). 



3.2.1 Naive Estimators 

The difficulty of applying standard estimators to RFSs arises because they 
represent information on the number of their elements (features) which is 
a dimensionless quantity, and the elements themselves, which can have di- 
mensions (in the case of features - their location, in units of distance from a 
globally defined origin) . To demonstrate some of the difficulties in deriving 
useful estimators for RFSs, consider the following example in which a PDF 
p() on the RFS J\A, representing an entire, unknown map, is assumed to be 
available. Intuitive, and standard, expected a posteriori (EAP) and maximum 
a posteriori (MAP) estimators are applied to a seemingly simple estimation 
problem [3J. 

Consider a simplistic situation in which there is at most one feature located 
in the map. Suppose that a corresponding feature existence filter 28J reports a 
0.5 probability of the feature being present. Suppose also that, if the feature 
is present, the posterior density of the corresponding spatial state, p(A4), 
indicates that it is equally likely to be found anywhere in the one-dimensional 
interval [0,2], with the unit of distance given in meters. It should be noted 
here that, already at this simplistic level, vector approaches cannot jointly 
model this feature state. Under an RFS representation however, the map 
state M. can be defined as Bernoulli RFS, with probability density, 



0.5 M = 
p(M) = { 0.25 M = {m},0< m< 2 . (3.1) 

otherwise 
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Note that the density, p(A4), is still a valid PDF, since its integral, with 
respect to M, equates to unity. However, in this case, it is difficult to define 
an expected a posteriori (EAP) estimate since the addition of sets is not 
meaningfully defined. Instead, a naive maximum a posteriori (MAP) estimate 
could be constructed as, 



M MAP = argsupp(TW) - 0, (3.2) 

M 



? 



(where = represents a question "Is it equal to?"), since p(0) > p({m}) 
(0.5 > 0.25). If the unit of distance is changed from meters to kilometres, 
the spatial probability density consequently becomes p(m) = U(0, 0.002), 
and the probability density of the map state A4 is, 

C 0.5 M = 

p(M) = < 250 M = {m},0 < m < 0.002 , 
[ otherwise 

and the naive MAP estimate then becomes, 

M MAP = argsupp(X) = {m} (3.3) 

M 

for any < m < 0.002 since p{{m}) > p(0) (250 > 0.5). This leads to 
the conclusion, that a target is now present, even though only the units of 
measurement have changed. This arises since the naive MAP yields a math- 
ematical paradigm which compares a dimensionlcss quantity p{M) (when 
M. = 0) to a quantity p(M) with dimensions (when M. = m). Such an MAP 
estimate is not well-defined since a change in the units of measurement re- 
sults in a dramatic change in the estimate. In fact the MAP is only defined if 
the units of all possibilities are the same, such as in discretised state spaces, 
divided into cells. 

This example has shown that standard estimators (EAP and MAP) are 
not well defined in the presence of non-unity target existence probability. It 
is therefore the aim of the next section to introduce new multi-target state 
estimators which arc well behaved. 



3.3 Bayes Optimal RFS Estimators 

Several principled solutions to performing multi-object state estimation are 
now presented, in the form of two statistical estimators and a first order 
moment technique (the PHD filter) with desirable properties. 

We begin by opening the discussion on the full SLAM problem in terms of 
joint Bayes optimal estimators for the vehicle trajectory and the map. The 
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Bayes risk is then defined for the map along with corresponding feature map 
estimators. Finally, Bayes optimal estimation approximations for FBRM and 
SLAM are derived. 

This section discusses various Bayesian estimators for the SLAM problem 
and their optimality, based on a vector representation of the vehicles trajec- 
tory, and a finite-set representation of the map. The notion of Bayes optimal 
estimators is fundamental to the Bayesian estimation paradigm. In general, 
if the function 9 : z i— > 9(z) is an estimator of a parameter 9, based on a 
measurement z, and C(9(z),9) is the penalty for using 9(z) to estimate 9, 
then the Bayes risk R(9) is the expected penalty over all possible realisations 
of the measurement and parameter, i.e 

R{9)= f [ C(9{z),9) P (z,9)d9dz (3.4) 



where p{z, 9) is the joint probability density of the measurement z and the 
parameter 9. A Bayes optimal estimator is any estimator that minimises the 
Bayes risk. 

In the SLAM context, relevant Bayes optimal estimators are those for 
the vehicle trajectory and the map. The posterior densities Pk(Xv.k) — 
Pk(Xuk\Zo:k,Uo:k-i,Xo) and p k (M k ) =Pk(M k \Z 0:k , U 0: k-i, X ) for the ve- 
hicle trajectory and map, can be obtained by marginalising the joint posterior 
density, Pk(-M.k,Xi:k\2o-.ki Uo : k-i 7 X n ). For the vehicle trajectory, the poste- 
rior mean, which minimises the mean squared error (MSE), is a widely used 
Bayes optimal estimator. However, since the map is a finite set, the notion 
of MSE does not apply. Moreover, standard Bayes optimal estimators are 
defined for vectors and subsequently do not apply to finite-set feature maps. 
To the best of authors' knowledge, there is no work which establishes the 
Bayes optimality of estimators for finite-set feature maps (and consequently 
feature-based SLAM). Therefore the following sections propose frameworks 
for Bayes optimal estimation with RFSs, which assume varying degrees of 
approximation to the statistical representations of sets. 



3.3.1 Bayes Risk in Feature Map Estimation 

The convergence of the vehicle location estimation aspect, of feature-based 
frameworks, has received a great deal of attention to date Q]. However, to 
the authors' knowledge, the convergence of the corresponding map estimate, 
particularly with regards to converging to the true number of features, has 
never before been addressed or proven. Therefore, to address the optimal map 
estimation problem, as before, let Mk denote the feature-based map state 
at time k comprising m fc features and pu{M-k) denote its posterior density. 



Note that henceforth for compactness, Pk{-) = Pk\k{-)- 
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If Mk ■ 2i : k i— > Mk{Zi-.k) is an estimator of the feature map M k , and 
C(Mk(Z 1: k),Mk) is the penalty for using M k {Zi- k ) to estimate Mk, then 
the Bayes risk for mapping is given by 

R(M k ) = I fc(M k (Z 1:k ),M k )p k (M k ,Z 1:k )SM k 5Z l:k . 

where p k (M k ,Zi :k ) is the joint density of the map and measurement his- 
tory. Note that since the map and measurements are finite sets, standard 
integration for vectors is not appropriate for the definition of the Bayes risk. 
Subsequently the Bayes risk above is defined in terms of set integrals. Sev- 
eral principled solutions to performing feature map estimation are presented 
next, with the main focus of attention being on the PHD filter in Section 
13.3.41 which is used widely throughout this book. The following estimators 
are Bayes optimal given the definition of an appropriate Bayes risk as just 
described. 



3.3.2 Marginal Multi-Object Estimator 

The Marginal Multi-Object (MaM) estimator is defined via a two-step esti- 
mation procedure. The number of features is first estimated using a maximum 
a posterior (MAP) estimator on the posterior cardinality distribution, p, 

m fe = argsupp fe (|.Mfc| = m ), (3.5) 

m 

Second, the individual feature states are estimated by searching over all maps 
with cardinality xh k , using a MAP criteria, 

— MaM 

M k = arg sup p k {M). (3.6) 

M:\M k \=m k 

It has been shown that the MaM estimator is Bayes optimal, however con- 
vergence results are not currently known. 



3.3.3 Joint Multi-Object Estimator 

In contrast to the MaM estimator, which first estimates the number of fea- 
tures and restricts its feature state estimation process to maps with only 
that number of features, the Joint Multi-Object ( JoM) estimator executes its 
feature state estimation process on maps of all possible feature number. 
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The JoM estimator is defined as 

Mif 1 = argsup p k (M k ) -r-^ , (3.7) 

where s is a constant with units of volume in the feature space, argsup 
denotes the argument of the supremum, and \M. k \ denotes the cardinality of 
A4 k . Notice that the fundamental difference between this estimator and the 
MAP estimator of eauations l3.2l and l3.3l is that the factor s! a ^' c I/|A / Ja;|! allows 
target based attributes of differing dimensions (e.g. spatial and non-spatial) 
to be "compared" in a principled manner. 

First, to consider all possible sizes m of the feature map for each m > 0, 
determine the MAP estimate, 

Al (m) -arg sup p k (M\Z 0:k ). (3.8) 

M:\M\=m 



Second, set 



_ ^ _ ^ o"l 

M JoM =M (m) where m = argsup Pk (M {m) \Z . k )— -. (3.9) 

m m! 

It has been shown that the JoM estimator is Bayes optimal and is statistically 
consistent i.e. the feature map error distance (to be discussed in Section 
14. 3|) . between the optimal estimate and the true map, tends to zero as data 
accumulates [23], [3J, [27]. Hence, 

• "The JoM estimator determines the number m and the locations M. of 
features optimally and simultaneously without resorting to optimal data 
association." [3J. 

Additionally, the value of s in equation 13.91 should be made equal to the 
desired accuracy for the state estimate. The smaller s is, the greater the ac- 
curacy of the estimate, but the rate of convergence of the estimator will be 
compromised. Because of this, while JoM is a theoretically attractive estima- 
tor, it is computationally expensive. 



3.3.4 The Probability Hypothesis Density (PHD) 
Estimator 

A simple approach to set-based estimation, is to exploit the physical intuition 
of the first moment of an RFS, known as its PHD or intensity function. 
This corresponds to the multi-feature equivalent of an expected value - the 
expectation of an RFS. 

This section starts by giving an explanation of what the PHD is, and how 
it should be statistically interpreted in Section 13.3.4. II This is followed by 
two intuitive derivations of the PHD in Sections 13.3.4.21 and 13.3.4.31 
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:;;{ 



3.3.4.1 Interpretation of the PHD 

The intensity function at a point, gives the density of the expected number 
of features occurring at that point and therefore the mass (integral of the 
density over the volume of the space) of the PHD gives the expected num- 
ber of features. The peaks of the intensity function indicate locations with 
relatively high concentration of expected number of features, in other words 
locations with high probability of feature existence. To provide an intuitive 
interpretation, consider a simple ID example of two targets located at x — 1 
and x = 4 each with spatial variance a 2 = 1 taken from page 569, [3]. A 
corresponding Gaussian mixture representation of the PHD for this problem 
is: 



PHD(il-) 



1 



2vrcr 



cxp 



2a 2 



cxp 



(s-4 ) 
2a 2 



2\ 1 



(3.10) 



PHD (a;) versus x is plotted in figure 1341 Note that the maxima of PHD(x) 
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Fig. 3.1 A PHD for a ID, 2 target problem of equation |3~TU1 



occur near the target locations (x — 1, 4)). The integral of PHD(x) is m 
where 

m= fpUD(x)dx= I N{l 1 o 2 )dx+ J N{^a 2 )dx (3.11) 

= 1 + 1 =2 
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i.e. m equals the actual number of targets. Here we note that a PHD is not a 
PDF, since its integral over the space of its variable is not, in general, unity. 
For a 2D, robotic feature based map, graphical depictions of posterior 
PHDs after two consecutive measurements, approximated by Gaussian mix- 
tures, are shown in figures [5T2"1 and [5751 In each figure the intensity function 




Fig. 3.2 A sample map PHD at time k—1, with the true map represented by black 
crosses. The measurement at k—1 is represented by the yellow dashed lines. The 
peaks of the PHD represent locations with the highest concentration of expected 
number of features. The local PHD mass in the region of most features is 1, indi- 
cating the presence of 1 feature. The local mass close to some unresolved features 
(for instance at (5,-8)) is closer to 2, demonstrating the unique ability of the PHD 
function to jointly capture the number of features. 




Fig. 3.3 A sample map PHD and measurement at time k. Note that the features 
at (5,-8) are resolved due to well separated measurements, while at (-3,-4), a lone 
false alarm close to the feature measurement contributes to the local PHD mass. At 
(-5,-4) a small likelihood over all measurements, coupled with a moderate Ck{z\Xk) 
results in a reduced local mass. 



is plotted as a function of the spatial coordinates. Since the integral of the in- 
tensity function (or PHD) is, by definition, the estimated number of features 
in the map, the mass (or integral) of each Gaussian can be interpreted as 
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the number of features it represents. In the case of closely lying features (and 
large measurement noise), the PHD approach may not be able to resolve the 
features, as demonstrated for the right hand feature of Figure I3~2l at approxi- 
mate coordinates (5, -8). However the PHD will represent the spatial density 
of L features by a singular Gaussian with a corresponding mass of L, which 
may improve the feature number estimate. This is only theoretically possible 
using the RFS framework. A graphical example for L = 2 is illustrated in 
Figure 13.21 which is then resolved through measurement updates into indi- 
vidual Gaussian components for each feature of mass L « 1, as shown in 
Figure |3~B1 (the two peaks at approximate coordinates (5, -8)). 

The PHD estimator has recently been proven to be Bayes optimal [55] and 
has been proven to be powerful and effective in multi-target tracking [3|. 

3.3.4.2 The PHD as the Limit of an Occupancy Probability 

Intuitively, the PHD can be derived as a limiting case of the occupancy 
probability used in grid based methods. Following [5DJ, consider a grid system 
and let rrii denote the centre and B(m,i) the region defined by the boundaries 
of the ith grid cell. Let P( occ \B(rrii)) denote the occupancy probability and 
\(B(rrii)) the area of the ith grid cell. Assume that the grid is sufficiently 
fine so that each grid cell contains at most one feature, then the expected 
number of features over the region Sj = (J B(rrii) is given by, 

ie.J 



E [\M n Sj\] = Y, P ( ° cc) {B{ mi )) 

= ^u(m,)A(B(m J )). (3.12) 

where v{rrii) = — \<B(m T) • Intuitively any region Sj can be represented 

by U B(rrii), for some J. As the grid cell area tends to zero (or for an 

ie.J 
infinitesimally small cell), S(m,) — ► dm. The sum then becomes an integral 
and the expected number of features in S becomes, 



E[|MnS|] = / v(m)dm. (3.13) 

Js 

v(m) defines the PHD and it can be interpreted as the occupancy probabil- 
ity density at the point m. The (coordinates of the) peaks of the intensity 
are points (in the space of features) with the highest local concentration of 
expected number of features and hence can be used to generate optimal es- 
timates for the elements of M. . The integral of the PHD gives the expected 
number of features and the peaks of the PHD function can be used as esti- 
mates of the positions of the features. 
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3.3.4.3 The PHD as the Density of the Expectation of a Point 
Process 

An alternative derivation of the PHD now follows. An analogous notion to 
the 'expectation' of an RFS can be borrowed from point process theory. This 
construct treats the random set as a random counting measure or a point 
process (a random finite set and a simple finite point process arc equivalent 

EB). 

Let p(M) be the multi- feature probability distribution of the map RFS 
M . A somewhat naive interpretation of its expected value M would then be 

M**y £ T M.p(M)6Mi. (3.14) 

where Mi represents the ith. subset of M. Since the addition of finite subsets 
of M is undefined, the above integral is also undefined. It can be solved 
by defining a transformation which maps finite subsets Mi into vectors M{ 
in some vector space. This transformation must maintain the set theoretic 
structure by transforming unions into sums - i.e. Mi U Mj = Mi + Mj, if 
Mi fl Mj = 0. In this case, an expected value can be defined in terms of the 
"equivalent" vectors 

M = E[M] = J Mip(M)dMi (3.15) 

The point process literature |32j uses a transformation Mj = 6 Mi where 

S Mi = if Mi = (3.16) 

^Mi = / S(x — m) otherwise. 

m£Mi 

where x is the vector space of the features and S(x — m) is the Dirac delta 
density concentrated at each feature m within the random finite subset Mi. 
Taking the expectation of equation 13.161 gives 

v(m) 4 R[6 M ] - / 8 Mi p{M)8Mi (3.17) 



which is the multi-feature equivalent of the expected value. This is called the 
probability hypothesis density (PHD), also known as the intensity density, 
or intensity function v(m) of M. 

Note that while v(m) is a density, it is not a PDF, since it may not nec- 
essarily integrate to 1. This is clear, as the integral of v(m) over any region 
S gives the expected number of features in that region - i.e. 

v(m)dm = E[\Mr\S\] (3.18) 

s 
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Hence, the integral of the non-negative intensity function v(m) over M. gives 
the expected number of features in the map. 

Note that we have arrived at the same result as equation 13.131 in which 
the PHD was considered to be the limit of an occupancy probability. 

3.3.4.4 Recovering the Map from the PHD Intensity Function 

Since v(m) is a density, it can be readily approximated by standard Sequen- 
tial Monte Carlo (SMC) or Gaussian Mixture methods as described later in 
Chapter SJ The PHD filter recursion therefore propagates the intensity func- 
tion v(m) of the RFS state and uses the RFS measurement in the update 
stage. Since the intensity is the first order statistic of a random finite set, the 
PHD filter is analogous to the constant gain Kalman filter, which propagates 
the first order statistic (the mean) of the vector-based state. However, the 
intensity, v(m), can be used to estimate both the number of features in the 
map, and their corresponding states (along with the uncertainty in the state 
estimates) [2]. 

If the RFS, A4k, is Poisson, i.e. the number of points is Poisson dis- 
tributed and the points themselves are independently and identically dis- 
tributed (IID), then the probability density of A4k can be constructed exactly 
from the PHD. 

II v k{m) 

Pk (M k ) = me *» (3.19) 

cxp(J Vk\ra)dni) 

where Vk{m) is the map intensity function at time k and Mk is the RFS map 
which has passed through the field of view (FoV) of the vehicle's on board 
sensor(s) up to and including time k. In this sense, the PHD can be thought 
of as a 1st moment approximation of the probability density of an RFS. 

Under these approximations, it has been shown [2J that, similar to standard 
recursive estimators, the PHD recursion has a predictor- corrector form. 



3.4 The PHD Filter 

As defined in Section f3. 2. 11 M. is the RFS representing the entire unknown 
map. Let Mk-i be the RFS representing the explored map, with trajectory 
X 0: k-i = [X ,Xi, . . . ,Xk-i] at time k - 1, i.e. 

M k -! = M n FoV(X ..k-i). (3.20) 

Note that FoV(X :k-i) = FoV(X Q ) U FoV(Xi) U • • • U FoV{X k ^{). M k -i 
therefore represents the set on the space of features which intersects with 
the union of individual FoVs, over the vehicle trajectory up to and including 
time k—1. Given this representation, Mk-i evolves in time according to, 
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M fe _iU (FoV(X k )nMk-i^j 



M k =M k - 1 ulFoV(X k )r\M k - 1 j (3.21) 

where M.k-1 = Ai — Aik-t represents the unexplored map (note the dif- 
ference operator used here is the set difference, sometimes referred to as 
.M\.Mfc-i or the relative complement of M. k -\ in Ai), i.e the set of features 
that are not in A^fe_i. Let the newly explored features which have entered 
the FoV be modelled by the independent RFS, B k {X k ). In this case, the RFS 
map transition density is given by, 

f M {M k \M k -i,X k )= J2 fM(yV\M k -i)f B (M k -W\X k ) (3.22) 

WCM k 

where /m (W|.Mfc_i) is the transition density of the set of features that are 
in FoV(Xo :k -i) at time k — 1 to time k, and fB{AA k —W)\X k ) is the density 
of the RFS, B(X k ), of the new features that pass within the field of view 
at time k. To define the PHD filter in a form general enough to be applied 
to FBRM and SLAM, we now define a state variable r k which corresponds 
to the state of interest to be estimated. In the case of FBRM, r k would be 
replaced by ii m,\X k , i.e. the feature at m, given the vehicle location X k . This 
implementation of the PHD filter will be the subject of Chapter 0J In the 
case of SLAM a "brute force" approach is implemented in Chapter [3] which 
replaces r k with each feature augmented with a hypothesised vehicle trajec- 
tory. Its implementation is shown to demonstrate a viable, and theoretically 
simple, SLAM implementation. A more elegant, Rao-Blackwellised imple- 
mentation of the PHD filter is implemented in Chapter [5] which propagates 
N conditionally independent PHDs, based on each of the N hypothesised 
trajectories, represented as particles. In this case, r k is effectively replaced 
by "m|Ao:fc" i.e. the feature at m conditioned on the vehicle trajectory X^ k . 
This will demonstrate a more computationally efficient SLAM implementa- 
tion, which allows the Bayes optimal, expected trajectory and expected map 
to be evaluated. 

In terms of the general state variable r k the prediction of the map intensity 
function Ufc|k-i(A), is given by 

Wk|k-i(.Tfc) - v fc -i|*_i(i*_i) + 6(r fc ) (3.23) 

where b(r k ) is the PHD of the new feature RFS, B{X k ). Note that u fc -i|fc-i() 
corresponds to the estimate of the intensity function at time k — 1, given all 
observations up to, and including, time k — 1. For ease of notation however, 
this will be referred to as v k -\() in future instances. 
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The PHD corrector equation is then [2J, 



vk(r k ) =Ufcifc_i(r fe ) 



1-Pd(A)+ 

\p Pp(r k )gk(z\r k ) 

z ^ k Cfc(z) + / PD(€k)9k(z\€k)vk\k-i(€k)d€k 



(3.24) 



where v k \k-i{Pk) is the predicted intensity function from equation 13.231 £ is 
a subset of .Mfc and, 

PD(r k ) = the probability of detecting a feature at 

m, from vehicle location X^ (encapsulated in r k ), 
gk(z\-Tk) = the measurement model of the sensor at time k, 
Ck(z) = intensity of the clutter RFS C k (X k ) (in equation 12. 10p at time k. 

3-4- 1 Intuitive Interpretation of the PHD Filter 

An intuitive interpretation of the PHD filter equations l3.23l and l3.24l is given 
in Chapter 16 of [3]. The predictor equation 13.231 comprises the addition of 
the previous PHD correction and the PHD of the set of features hypothesised 
to enter the sensor's FoV. The corrector equation 13.241 can be more clearly 
interpreted in its integrated form since, by definition 

v k (r k )dr k =m k (3.25) 

where trtfc is the number of estimated features at time k. To simplify the 
interpretation further, a constant (state independent) probability of detection 
is assumed in this section - i.e. 

PD{r k ) = P D - (3.26) 

Therefore, from equation 13.241 



m fc = / v k (r k )dr k 

= (1 - P D )m k \k-i + 

p \p J,9fc(z|AK|fc-i(A)dA 

D h <*(*)+ iVs*(*i&w| fe -i(&)d& [ ' 

Notice that the integrals in the numerator and denominator of the final term 
within the summation of eouation l3.27l are identical and to simplify the equa- 
tion we introduce 
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D k \ k -i(gk,v k \k-i) - / 9k(z\rk)v k \k-i( r k)dr k = / 9k(z\^k)v k \ k -i(^k)d^ k 

(3.28) 
where g k abbreviates g k (z\r k ) and Vk\k-i abbreviates Vk\k-i(r k )- Therefore 
the integral of the PHD corrector equation 13.241 with constant Pp , can be 
written as the feature number corrector equation 






Adfe-iCfffci^fclfc-i) 



m fe = (1 - P D )m k]k -i + Pd}^ -, ; , ' " T (3-29) 

*-i c k {z) + PDD k \ k _i(g k ,v k \ k _i) 

Equation 13.291 is useful for intuitively interpreting the PHD corrector equa- 
tion, and is governed by the following effects: 

1. Probability of detection Pjj. If the map feature at m is not in the FoV of 
the sensor, it could not have been observed, thus Pn = 0. Therefore, from 
equation 13.291 

m k = (l-0)m fe | fe _i +0 = m fe | fe _i (3.30) 

i.e. the updated number of features simply equals the predicted number, 
since no new information is available. Similarly from from equation 13.241 

v k (r k ) = vfc|fc_i(/ik)[i - o + o] = w fc | fc _i(r fc ) (3.3i) 

i.e. the updated PHD will simply equal the predicted value. 

On the other hand, if m is within the sensor FoV and if Pjj w 1, the 

summation in equation 13.241 tends to dominate the PHD update and 

9k{z\r k ) 



v k (r k ) w w fc |fc_i(r fe ) 



E 



c k{z) + J gk{z\£ k )v k \ k -i{i k )d£, k 



(3.32) 



Then the predicted PHD is modified by the sum of terms dependent on 
the measurement likelihood and clutter PHD. 

2. False alarms c k (z). A particular feature observation could have originated 
from a feature or as a false alarm. Assume that the number of false alarms 
A (represented by its intensity c k (z)) is large and uniformly distributed 
in some region R. If the observed feature is in _R, the term within the 
summation of equation 13.291 becomes 



, Dk\k-l{9k,Vk\k-l) 

Cfc(z)+P D £ ) fc|fc-l(fffe,^fe|fc-l) 

„ Dk\k-i(9k,v k \k-i) \R\ 

P D x ; ~ n ~ PD—Dk\k-i(9k,Vk\k-i) 

t^t + r , D^k\k-l{gk,Vk\k-l) 



(3.33) 



A 



since A is so large that it dominates the denominator. Therefore, if an 
observation originates from R, it is likely to be a false alarm, and it con- 
tributes almost nothing to the total posterior feature count, as it should. 
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On the other hand if a measurement originates from a region other than 
R, with low clutter, then it is unlikely to be a false alarm. This means that 
Cfc(z) ~ and 

Dk\k-i(gk,Vk\k-i) ^ D k \ k _i(g k ,v k \ k _i) 

c fe (z) + P D L> fe | fe _ 1 (5 fe ,w fc | fc _ 1 ) ~ + Pr»-Dfe|fc-i(5fc,«fe|fe-i) 

(3.34) 
so that the measurement now contributes one feature to the total feature 
number. In general, if the number of false alarms, governed by the clutter 
PHD c k (z\X k ), is high, this increases the denominator of the summation, 
thus lowering the effect of the sensor update, as it should. 

3. Prior information PD9k{z\r k ). Assume that the sensor model is good, and 
Po9k{z\r k ) is large for a particular state r k which produces z. If z is consis- 
tent with prior information (the observation model), PoD k \ k -i(g kl ffclfc— l) 
will tend to dominate the denominator of the summation in eauation l3.29l 
and the term corresponding to that feature in the summation will become 

P D - D ^-^k,v k \ k -i) w 1 (3 35) 

c k (z) + P D D k \ k _i(g k ,v k \ k _i) 

Hence, a feature which is consistent with the observation model tends to 
contribute one feature to the total feature count. 

Conversely if the observation z is inconsistent with prior information (is 
unlikely according to the sensor model), then the product PDgk{z\r k ) will 
be small, and its corresponding term in the summation in equation 13.291 
will tend to be ignored. 

Equations 13.231 and 13.241 which comprise the PHD filter have been shown 
to be Bayes optimal, assuming that the RFS observation and map statistics 
can be represented by their first moments only [3]. 



3.5 Summary 

This chapter addressed the issues of estimation with RFSs. Initially, the tra- 
ditional MAP and EAP estimators were applied to a simple, single feature 
problem with both feature existence and spatial uncertainty. It was demon- 
strated that such estimators are not suitable in such applications, and new 
multi-feature estimators were defined, which minimised the Bayes risk in fea- 
ture map estimation. 

The main focus of attention of the chapter was on the PHD filter. An RFS 
map density can be represented by its first moment, the intensity function. 
Brief derivations for the PHD estimator (intensity function) were shown based 
on the PHD as the limit of an occupancy probability, and the density of the 
expectation of a point process. 
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The PHD recursion is far more numerically tractable than propagating the 
RFS map densities of eauation l2.15l In addition, the recursion can be readily 
extended to incorporate multiple sensors/vehicles by sequentially updating 
the map PHD with the measurement from each robot. 



Part II 
Random Finite Set Based Robotic 

Mapping 



Chapter 4 

An RFS Theoretic for Bayesian 

Feature-Based Robotic Mapping 



4.1 Introduction 

Estimating a FB map requires the joint propagation of the FB map density 
encapsulating uncertainty in feature number and location. This chapter ad- 
dresses the joint propagation of the FB map density and leads to an optimal 
map estimate in the presence of unknown map size, spurious measurements, 
feature detection and data association uncertainty. The proposed framework 
further allows for the joint treatment of error in feature number and loca- 
tion estimates. As a proof of concept, the first-order moment recursion, the 
PHD filter, is implemented using both simulated and real experimental data. 
The feasibility of the proposed framework is demonstrated, particularly in 
situations of high clutter density and large data association ambiguity. This 
chapter establishes new tools for a more generalised representation of the 
FB map, which is a fundamental component of the more challenging SLAM 
problem, to follow in Part II. 

In this chapter, FB map only estimation is addressed, i.e. the vehicle tra- 
jectory is assumed known. The chapter presents new insights to motivate an 
RFS approach, under which Baycs optimality is examined. Further, the con- 
cept of FB map estimation error (in the general case of an unknown number 
of features) is addressed through a mathematically consistent!] error metric. 

The chapter is organised as follows. Section l¥^l rciteratcs the natural repre- 
sentation of the feature map as a set of features, and formulates the Bayesian 
estimation problem for estimating the feature locations and number in a joint 
manner. Current FB error evaluation methods are discussed and a mathemat- 
ically consistent error metric introduced in Section [4.3l Section [4.41 explicitly 
relates the PHD filter concepts of section 13.41 in Chapter [3] to the FBRM 
problem. As a proof-of-conccpt, the first-order moment PHD recursion for 



1 A mathematically consistent metric is one that is defined on the state-space of 
interest (the space of all possible feature maps) and satisfies the necessary metric 
axioms. See Section [4JJI and [23] for further details. 
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FB maps is further outlined in Section EOl followed by contrasting filter im- 
plementations in Section [4.5l Simulation and real experimental results, using 
the new framework, are demonstrated in Section fOfl with comparisons to clas- 
sical approaches. Finally Section l4~8l offers some bibliographical comments on 
related robotic mapping work and summarises the latest developments in a 
related field of multi-object filtering. 

4.2 The Feature-Based Map Estimation Framework 

As introduced in Chapters [2] and [31 in contrast to the classical vector rep- 
resentation, a RFS map state, M k , can jointly encapsulate feature number 
and location uncertainty. Equivalently, a RFS measurement, Z k , can model 
uncertainty in measurement number and value. 

Given the current vehicle state, X%, and the feature RFS map Ai k , as 
mentioned in Chapter El (equation 12. I'D)) , and repeated here for convenience, 
the measurement consists of a set union, 

Z k = (J V k (m,X k )UC k (X k ) (4.1) 

meM k 

where T> k (m,X k ) is the RFS of a measurement generated by a feature at 
m and C k (X k ) is the RFS of the spurious measurements at time k, which 
may be dependent on the vehicle pose, X k . Therefore Z k consists of a ran- 
dom number of measurements. In this chapter, and throughout the book, 
the measurements take the form of range and bearing. It should be noted 
however that the RFS framework can be readily extended to other measure- 
ment types. Note that the number of detected measurements may differ from 
the number of features in M. k due to detection uncertainty, occlusion and 
spurious measurements. It is also assumed that T> k {m,X k ), and C k (X k ) are 
independent RFSs. 

The RFS of a measurement generated by a feature at m is assumed to be 
a Bernoulli RFqj given by, V k (m,X k ) = with probability 1 — Pr>(m, X^) 
and T> k (m,X k ) = {z} with probability density Pr>{m, X k )g k (z\m, X k ). For a 
given robot pose X k , Pr>{m,Xk) is the probability of the sensor detecting a 
feature at m and, when conditioned on g k (z\m, X k ), is the likelihood that a 
feature at m generates the measurement z. The concept of detection prob- 
ability is important to the measurement model, and indeed multiple models 
such as uniform or exponential mixture j33j can be accommodated in to the 
framework. In particular, state-dependant detection probabilities are most 
useful in a mobile robotics framework, since the ability of the sensor (or fea- 
ture extraction algorithm) to detect a given object can be highly influenced 



2 The Bernoulli RFS is empty with a probability 1 — a and is distributed according 
to a density p with probability a. The example in Section 13.2.11 was that of a 
Bernoulli RFS. 
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by its relative location. For instance, occlusions would effectively result in a 
Pd of zero, and it is also possible that the Pd would taper off with increasing 
distance from the sensor. While such approaches can be readily incorporated 
into the measurement model and subsequent filter [33], [34], throughout the 
context of this book a binary approach is taken. That is PD(m,X k ) = Pd 
when the feature is within the sensor field of view and Pr){m,X k ) = 
otherwise. 

The measurement generated by the sensor at time k is modelled by the 
RFS of eauation l4.ll The RFS Z k therefore encapsulates all measurement un- 
certainty such as measurement noise, sensor field of view (i.e. state-dependent 
probability of detection) and spurious measurements. The probability density 
that the sensor produces the measurement Z k , given the vehicle state X k and 
map Mk at time fc, is then given by the convolution [2], 

g k (Z k \X k ,M k ) = Y, gv(W\M k ,X k )g c (Z k - W). (4.2) 

wcz k 

Here, gv{yV\M k ,X k ) denotes the density of the RFS V k of measurements 
generated by features in A4 k , given the state of the vehicle, and gc(Z k — - W) 
denotes the density of the RFS C k of spurious measurements where, as in 
Section EOl the difference operation used in eauation l4.2l is the set difference. 
gv(W\M k ,X k ) describes the likelihood of receiving a measurement from the 
elements of the map, and incorporates detection uncertainty and measure- 
ment noise, gc (Z k — W) models the spurious measurements of the sensor and 
is typically a priori assigned [TB], |14j . 

If /x(A / (fc|A / (fe_i,Xfe_i) then represents the RFS feature map state tran- 
sition density (which typically incorporates an increasing number of map 
features as the vehicle moves, as shown in Section l3T4]l . and the RFS mea- 
surement is as in equation l4.ll the generalised Bayesian RFS FBRM recursion 
can be written in a form similar to equations l2.14l and l2.15l in Chapter [2] The 
difference here is that only the RFS map density is to be estimated: 



Pk\k-i{Mk\Zo :k -i,X k ) = J f M (Mk\Mk-i,X k )x 

Pk-l(M k -l\Zo: k -l,X : k -l)8M k -l (4.3) 



, ul3 v x 9k(Zk\X k ,Mk)pk\k-i(Mk\Zo:k-i,X k ) . . 

p k yM. k \Z-Q- k , X 0:k ) = -j — — — — — - (4.4) 

J 5fc(2 ; A : |X fc ,7W fc )p fc | fc _ 1 (7W fe |2o : fe-i,X fe )(5A^ fc 

where S once again implies a set integral. Note again here, that for ease 
of notation, p k -\ in equation 14.31 actually corresponds to Pfc— life— l an d p k 
in equation 14.41 actually corresponds to p k \ k . This simplified notation will be 
adopted throughout this book. Integration over the map, requires integration 
over all possible feature maps (all possible locations and numbers of features). 
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By adopting an RFS map representation, integrating over the map becomes 
a set integral, which can be defined as [3], 

Pk\k-i{Mk\Zo:k-i,X k )5Mk = 

°° 1 f 
Y, — ] Pk\k~i{W,...,m m "}\Zo: k -i,X k )dm 1 dm 2 ...dm mk . (4.5) 

m fc =0 mfc ' J 

This feature map recursion therefore encapsulates the inherent feature num- 
ber uncertainty of the map introduced by detection uncertainty, spurious 
measurements and vehicle manoeuvres, as well as the feature location uncer- 
tainty introduced by measurement noise. Features are not rigidly placed in 
a map vector (as is the case in previous approaches), nor arc measurements 
simply a direct function of the map state, due to the explicit modelling of 
clutter. Therefore, as introduced in Chapter contrary to previous vector 
represented approaches, no explicit measurement to feature assignment (the 
data association problem) is required. 

4.3 FB Map Estimation Error 

This book focuses on arguably the most popular and widely studied mobile 
robotics problem, localisation and mapping in a feature-based map. While 
in practice, the form a map takes would mostly depend on its application 
(path planning for instance would likely require higher resolution Grid Based 
(GB) maps), feature based maps form the basis of numerous mobile robotic 
frameworks. Whether for the given application, the feature represents a fixed 
static object for localisation, a target for defence or an injured human for 
search & rescue, the problem can in general be formulated as one of estimating 
an unknown number of objects at unknown locations. As such, a measure of 
the accuracy of any given mapping filter is of critical importance. To the 
authors' knowledge, there is currently no work which presents a well defined 
mathematical distance to gauge feature-based mapping error. 

The primary difficulty in determining map estimation error is due to dif- 
ferences between the estimated and true number of features, and the need to 
satisfy the 4 metric axioms 23jf|. Error metrics for fixed dimension problems, 
such as a sum of the squared error can be readily obtained from grid-based 
robotic frameworks [7] or metrics based on the Hausdorff distance 35 from 
the template matching literature. Such metrics however, cannot encapsulate 



3 The 4 metric axioms can be defined as follows. Let X be an arbitrary, non-empty 
set. Then the function d is a metric iff: 1) d(x,y) >= 0, for all x,y £ X; 2) 
d(x,y) = iff x = y, x € X (identity axiom); 3) d(x,y) = d(y,x), for all x,y £ X 
(symmetry axiom); 4) d(x,y) < d(x,z) + d(y,z), for all x,y,z £ X (triangle 
inequality axiom). 
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cardinality estimation error and are thus not suited to the evaluation of FB 
map estimates. 

A finite set representation of the map naturally allows for a metric to 
gauge map estimation error |23j . While there are several metrics for finite- 
set- valued estimation |23j , the feature map estimation error metric used here 
(which jointly considers errors in feature location estimate, and feature num- 
ber estimate) is based on a 2 rad -ordcr Wasserstein construction. If \M\ > \M\, 
it is given by, 



d^(M,M):= (t^(. nfin Vd( c >(m^) 2 +c 2 (|^M.M|) 



M\ N N 1/2 

y\M\ y 3 e{iZT\M\}- _ 

(4.6) 
where, 

d {c) {m\m j ) = min(c,||m i -m J '||) (4.7) 

is the minimum of the cut-off parameter, c, and the Euclidean distance be- 
tween the estimated feature location, fh l and the true feature location m 3 . If 
\M\ < \M\ the metric is obtained through d^ c \M, M). Note that the stan- 
dard Euclidean distance used in the definition of S c ^ (m l , m?) is arbitrary. To 
incorporate orientated features, other vector distances such as a Mahalanobis 
distance could be adopted where, 

d {c \m\m j ) = min (c, ((m l - m j ) T Pr 1 (fh i - m 3 )) 1/2 ) (4.8) 

with Pi being the estimated covariance of the i th feature. The Wasserstein 
construction is commonly adopted in theoretical statistics as a measure of 
similarity between probability distributions. It has been shown previously 
that for distributions of equal dimensionality, the Wasserstein distance re- 
duces to an optimal assignment problem [23] . Minimisation of a global dis- 
tance between the estimated and ground truth maps inherently considers the 
problem in its joint-estimation framework, as opposed to analysis of individ- 
ual estimates of features assigned through nearest neighbour methods [I] . The 
metric of equation 14. 61 explicitly accounts for the dimensionality estimate er- 
rors through the term c 2 (|Al | — \M\), which assigns a user-defined cost, c, for 
each unassigned feature state. This threshold decides whether a given feature 
estimate is an incorrectly declared feature, or is a correctly declared feature 
that is poorly localised. The effects of varying the c parameter are illustrated 
later in Section 14.6.51 For example, given the estimated maps of Figure [231 in 
Chapter^ with the parameter choice c = 5, d^ (A4i e ft,M.) = 1.48m whereas 
<J (5 ) (Mright, M) = 2.02m, with d^^M, M) = 0m. With such a metric, the 
quality of estimated maps can be compared in a quantitative manner. 

Equation 14.61 will be used to gauge the FBRM estimation performance, in 
the cases of known ground truth maps, in the results section. The remaining 
sections of this chapter are devoted to a solution to the FBRM problem using 
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the PHD filter. Contrasting RFS FBRM filters arc outlined and derived for 
both static and pseudo-static maps, coupled with analysis and results using 
benchmark approaches adopted by the robotics community. 

4.4 The PHD-FBRM Filter 



The recursion of equations 14.31 and 14.41 which are the mapping only versions 
of the general Bayes update equations 12. 141 and 12. 151 constitutes a Bayesian 
approach to the FBRM problem. However, as is the case for the vector recur- 
sion, a direct implementation is generally computationally intractable due to 
multiple integrals on the space of features. This is evident from the set inte- 
gration result of eauation l4.5l Therefore, as an approximation, the PHD filter, 
introduced in Chapter [3] (Section 13. 4ft , is used to propagate the expectation 
of the map as opposed to the full multi-feature posterior density. 

As introduced in Chapters[5]and[31 the newly explored features which have 
entered the FoV of the vehicle's sensor (s) are modelled by the RFS, B k (X k ) 
and the spurious measurements at time k, which may depend on the vehicle 
pose X k , are modelled by the RFS C k {X k ). Then, the state of interest in the 
FBRM case is the feature position m given the robot pose Xk- Hence in the 
general state update equations 13.231 and 13.241 

A — > m\X k (4.9) 

and from cauation l3.23l thc prediction of the map intensity function v k \ k _i (r k ) 
becomes v k \ k ^i(m\X k ) 7 given the robot location (i.e. FBRM), and is given 
by 

v k \k-i(m\X k ) = v k -i (m|X fe _ 1 j3+ b k {m\X k ) (4.10) 

where bk{m\Xk) is the PHD of the new feature RFS, B(Xk). 

The PHD filter corrector equation 13.241 Vk(rk) then becomes Vk(m\Xk), 
where 



v k (m\X k ) = v k \k-i(m\X k ) 1 - P D (m\X k ) + 

Pp(m\X k )g k (z\m, X k ) 

__ Zk c k (z\X k ) + f P D (Z\Xk)g k (z\Z,Xk)vk\k-i(Z\X k )dZ 

where v k < k _i(m\X k ) is the predicted intensity function and, 



(4.11) 



4 As mentioned in chapter [3j Vfc— 1() is a shortened notation, actually referring to 
v k -i\k-lO i- e - the estimated intensity function at time k— 1, given all observations 
up to, and including, time k — 1. 
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Pjj(m|Xfc) = the probability of detecting a feature at 

m, from vehicle location Xk-, 
gk{z\m, Xk) = the measurement model of the sensor at time fc, 
Ck(z\Xk) = intensity of the clutter RFS Ck(Xk) at time k. 

Hence the FBRM filter estimates Vk(m\Xk) which, under a Gaussian mixture 
(GM) implementation, has the form of the PHD (intensity function), shown 
in figures 13.21 and 13.31 In the FBRM case, under this representation, each 
Gaussian is conditioned on the known vehicle state Xk ■ 

While equation 14.111 defines the PHD measurement update, the follow- 
ing subsections describe differing interpretations of the map state, Mk- One 
interpretation is as a static (in both feature location and number) unknown 
state, while the other is as a static (in location), but monotonically increasing 
in number as the vehicle manoeuvres. 



4.4.I Static Map State 

The PHD recursion of equation 14.111 estimates the map by fusing the RFS 
measurement likelihood with the prior map intensity function. This represents 
a direct implementation of the FBRM problem, where a static map is assumed 
as M. = {m 1 , • • • , m mfc }, where trtfc is the true number of features in the entire 
map state. The problem is to estimate the entire number of map features rrifc, 
as well as their respective locations, m. Since the map, M., docs not evolve 
in time, the predicted multi-feature density is simply obtained by, 

Pfclfc-lGMl-Zork-l.-Yk) =p fc -i(M|2b:*-i,Jffc-i) (4.12) 

and consequently the predicted intensity required in equation 14.111 is given 
by, 

Vk\k-i(m\ x k) = v k -i(m\Xk-i) (4-13) 

where Vk—i(i r n\Xk—i) is the posterior intensity at time k — 1. This "brute 
force" approach to the FBRM problem requires an intuitive prior on the 
map state (analogous to the OG mapping problem), which is then refined 
through successive measurement updates. 



4-4- % Pseudo-static Map State 

A second contrasting interpretation of the map is to consider a map state 
with stationary features, but which grows indefinitely over time due to the 
limited sensor field of view. This pseudo-static map model concept was al- 
ready introduced in Section 13.41 and is repeated here for completeness. Let 
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the map state represent the subset of M, which has been observed by the 
on-board sensor, i.e. 

M k -i =Mn FoV(X ;k-i) (4.14) 

with FoV(X ;k-x) = FoV(X )UFoV(X 1 )U- ■ -Ufo^AVi) where FoV refers 
to the field of view of the sensor. Aik-i therefore represents the set on the 
space of features which intersects the union of the individual FoVs over the 
vehicle trajectory up to and including time k— 1. Given this representation, 
although the features in the map state are assumed static, the map itself 
evolves in time according to, 



M k = M k -! U I FoV(X 0:k ) n M k -A (4.15) 

where, Ad k -i represents the unexplored mapQ This predictive map model 
was introduced in Chapter [3l Section 13.41 This models the map state as 
static in value, but monotonically increasing in cardinality with time, due 
to new features in M. entering the field of view of the sensor as the vehicle 
traverses the terrain. Therefore, A4 k = {m 1 , • • • , m mk }, where rrifc is the time 
varying cardinality of M. k and trtfc < m, with m being the total number of 
features in the entire map, M. 

As introduced in section |3T4"1 the new features which have entered the FoV 
can be modelled by the RFS B(X k ) and the RFS map transition likelihood 
required in the recursion equation 14.31 can be written, 



f M (M k \M k -i,X k )= J2 fM(yV\M k -i)f B (M k -W\X k ) (4.16) 

WCM k 

where /m (W|.Mfc_i) is the transition density of the set of features that are 
in FoV(Xo- k ^i) at time k — 1 to time k, and fei-Mk — W|Xfe) is the density 
of the RFS, B(X k ), of the new features that pass within the field of view at 
time k. 

It can then be shown that the intensity of the multi-feature predicted 
density p(M k \Z 0:k -i,X k ) is given by [3], 

v k \ k - 1 {m\X k ) = v k _ 1 {m\X k - l ) + b k {m\X k ) (4.17) 

where bk{m\X k ) is the intensity of the new feature RFS, B{X k ). 

4.5 PHD-FBRM Filter Implementations 

This section outlines suitable implementations of the previously presented 
PHD-FBRM frameworks. The static map state assumption is analogous to 



Mathematically: all features in the entire map, M, not already in M k -i- 
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grid mapping approaches, in that an intuitive prior is set over the entire 
map, and inferences are drawn on both the observed and unobserved map. A 
Sequential Monte Carlo (SMC) implementation for this map interpretation is 
adopted. The pseudo-static approach is analogous to the approach adopted 
by the majority of FB map estimation algorithms pQ, [3B], [T7], which uses 
Kalman filters for map estimation. For accurate comparison with previous 
vector based methods, a Kalman-based, GM implementation is thus adopted. 



4.5.1 The Static Map: An SMC PHD-FBRM 
Implementation 

Recall from section 14.4.11 that the the static feature map filter is analogous 
to an occupancy grid in that the number of map elements is fixed a priori. 
Dispersing particles in a grid (or any arbitrary) layout, represents all the pos- 
sible regions of features occurring, whereas the total weight in a given region 
represents the a priori estimate on the number of features in that region. 
Thus in this map representation, the particles model both the multitude of 
the number of features in the map, as well as their locations. 

This section presents a SMC implementation of the static map PHD- 
FBRM recursion. Assume at time k— 1, a set of weighted particles 
{&!?_!, i^k-i} i^i 1 representing the prior intensity, Vk-i(m\Xk-i) is available, 

i.e. 

Jk-i 

^xMavo » y, 4-i^\( m )- ( 4 - 18 ) 

Exploiting a suitably chosen proposal q, the predicted MC approximation for 
particles j = 1, . . . , 3k— \ can be obtained according to, 

ro£°~g(m|ro£2i,.Z*) (4.19) 

According to the PHD-FBRM update of equation 14.111 following the mea- 
surement Zk at time k, the posterior map intensity may then be approximated 
by, 

Jk 

« fc (m|A fc )«^4 j) <5 m o,(m) (4.20) 

i=i 
where, based on equation 14. Ill the weights are initially calculated via, 



,~,U) 



1 - P D {mf\X k ) + £ fefrgU*) 



Ck 



; (z) + £, 2 (mi 4) ,A fe )c« 1 



,U) 



"£i ( 4 - 21 ) 
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with, 

Sz(rf$\x k ) = P D (m^\X k )g(z\m k j \x k ). (4.22) 

Resampling then results in the required posterior intensity parameters, 
{mj, ,wj, }■'[. Note that contrary to standard SMC implementations, the 
sum of the weights U! k used to calculate the updated intensity function of 

cauation l4.20l is not unity. In this case, the sum Ylj=i w ! = ™-k, the estimated 
dimensionality of the feature map. 

4.5.1.1 State Initialisation and Estimation 

The static map FBRM implementation requires the map intensity to be a 
priori initialised. This is analogous in principle to grid mapping algorithms, 
in which the occupancy state is typically initialised at a non-informative value 
of 0.5. For the FBRM case, a non-informative initialisation condition is an in- 
tensity with uniform spatial distribution over the entire mapping state space. 
The integral of the intensity would give the expected number of features 
within the map. This can be achieved through an expected feature density, 
however, in practice, the results are not overly sensitive to this initialisation 
parameter. At any time k, the posterior map estimate can be extracted by 
segmenting the posterior particle approximation into m k clusters (perhaps 
with a K-means clustering algorithm) and using the cluster centroid as the 
estimate of the feature location. 

4.5.1.2 Particle Representation Considerations 

Unlike particle filter implementations for robot localisation filters [37) , main- 
taining a fixed number of particles for a robotic mapping filter may result 
in depletion issues where there is an insufficient number of particles to ade- 
quately represent the spatial uncertainty of all estimates features. Therefore 
the map is typically rcsamplcd with respect to p, the number of particles 
per feature, to result in J k = pm k - Furthermore, a static particle transition 
(equation 14.191) may deteriorate the accuracy of the filter since the measure- 
ments would not be bound to the initial particle lattice (such as is the case for 
GBRM filters) . A Brownian transition of very small covariance can therefore 
improve estimation accuracy. 

In practise, the static-map FBRM filter can be prone to large dimensional- 
ity estimation errors, especially in the case of features with non-unity detec- 
tion probability. In such cases, the following pseudo-static implementation 
is better suited, as the transition equation directly accounts for predicted 
dimensionality changes in the map, for instance as a result of features only 
being detectable well within the sensor field of view. 
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4.5.1.3 Static Map, SMC PHD FBRM Pseudo Code 

This section details the pseudo-code of the proposed filter. Table |4~T1 outlines 
in the initialisation algorithm while Table 14.21 describes a simple prediction 
algorithm assuming a static map. Table 14.31 shows how to update the map 
given the latest measurement and vehicle pose. Tables l4~4l and 1431 detail the 
resampling and estimation processes of the filter. 



Table 4.1 SMC-PHD-FBRM-Initialise 



Algorithm SMC-PHD-FBRM-Initialise(w (m|X )) 

// For all map particles 

1. for i = 1 to J do 

// Initialise strategy (e.g. grid lattice) 



2. 


M r i 

m o — i x y\ 


// 


Estimate of feature density 


3. 


(i) 


4. 


end for 


// 


The initialised static map PHD 


5. 


v (m\X ) = {^ ) ,m$ ) }£ 1 


6. 


return (vq (jn\ Xq ) ) 



Table 4.2 SMC-PHD-FBRM-Predict 



Algorithm SMC-PHD-FBRM-Predict(« fc _i(m|X fc _i)) 

// PHD Prediction 

1. for i = 1 to Jk-i do 

// Static map assumption with perturbation 

2. fh k = ra^_i + 5{m) 

3. end for 

4- Jk\k-l = Jk 

II The predicted map PHD 

5.v klk _ 1 (m\X k ) = {u;«l 1 ,m ( J ) } J j h Jr 
6. return(u fc | fc _ 1 (m|X fc -i)) 
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Table 4.3 SMC-PHD-FBRM-Update 



Algorithm SMC-PHD-FBRM 


-Update ( 


2*>- 


Xk,V k 


fe _i(m|X fe _ 


)) 


//To obtain v k (m\X k ) 












1. for j = 1 to lu do 












2- ?f = E£r^(s;# fe m« 


*)«& 










3. end 












// evaluate (|4.21|) 












4. for i = 1 to Jfc|fc_i do 












5. u>® = (l - P D (m\X k ) + 


w ) w k-i + 


3fc 


N(z' 


;H k m k l) ,R) 


41 






.7 






6. end for 












7.v k (m\X k ) = {Q^\m^ k Jr 












8. return(i;fc(m|Xfe)) 













Table 4.4 SMC-PHD-FBRM-Resample 



Algorithm SMC-PHD-FBRM-Resample (i7 fe (m|X fe _i)) 
// Resample the updated PHD 

i. a, = x: 4 j) 

// Normalise weights for resampling 

2. Resample {^.m^}/*?- 1 

3. Rescale resampled weights by rhfc to get {oJ k li,m k }jLi 

4. v k {m\X k ) = {J k j \m k j) } J ^ x 

5. return(ufc(rTi|Xfc)) 



Table 4.5 SMC-PHD-FBRM-Estimate 



Algorithm SMC-PHD-FBRM-Estimate (v k (m\X k )) 
II Initialise the map estimate 

1. M k = 

2. Cluster {m^'}^ into m^ clusters, e w 
2. for i = 1 to rrifc do 

// concatenate estimate 

4. 7W fe = [M k centroid(e«)] 

6. end for 

7. return (M k ) 
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4.5.2 The Pseudo-static Map: A GM PHD-FBRM 
Implementation 

This section presents a Gaussian mixture implementation of the pseudo-static 
map PHD-FBRM recursion. Recall from section l4.4.21 the pseudo-static map 
is a map which is modelled as static in feature location, but monotonically 
increasing in size as new features are observed. As with the SMC implemen- 
tation described in section 14.4.11 the Gaussian components of the mixture 
represent the multitude of locations of features in the map, while their masses 
represent the number of features in that given region. In this case, let the 
prior map intensity, Vk-i, be a Gaussian mixture of the form, 

Jk— 1 

^Havi) = E *£-i^(™;/#2i.-pfc-i) ( 4 - 23 ) 



which is a mixture of Jk-i Gaussians, with w^\, Mfc— 1 an d Pff-i being their 
corresponding predicted weights, means and covariances respectively. Note 
that, in contrast to the previous implementation of Section 14.5.11 the map 
state is now time dependent. Let the new feature intensity in equation l4.17l 
bk(rn\Zk~i, -Xfc_i) at time fc, also be a Gaussian mixture of the form 

Jb,k 

b k (m\Z k ^, X k _ x ) = J2 «#M m ; /$, P$) (4.24) 



where, </(,.& defines the number of Gaussians in the new feature intensity at 
time k and w^ k , n^' k and P fe ^ determine the shape of the new feature GM 
proposal density according to a chosen strategy. This is analogous to the 
proposal distribution in the particle filter and provides an initial estimate of 
the new features entering the map. More details on the chosen strategy are 
provided in Section [4.5.2.11 The predicted intensity vu k _i is therefore also a 
Gaussian mixture, 

Jk \k - 1 

t*| fc -i(m|X fc )= J2 Uk&-M m '>»kl-x> p !&-i) ( 4 - 25 ) 

3=1 

which consists of Ju^-i = Jk— 1 + Jb.k Gaussians representing the union of 
the prior map intensity, Vk-i(m\Xk), and the proposed new feature intensity 
according to equation 14. 171 where. 
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«$-! = «& 

A*fe|fe-i = Mfc-i ^ or ■?' *={!;■•■' ^fe-i} (previously observed features) 

p(i) _ pU) 
r h\h-l ~ r k-l 

W k\k-1 — W b,k 

nc\k-l = n!,k k ~ } ior 3 e {( J fc-i + 1 )> • • ■ > Jfc|fc-i} (newly observed features). 
pO) _ pU-Jk-i) 

Since the measurement likelihood is also of Gaussian form, it can be seen 
from equation 14.111 that the posterior intensity, v k is then also a Gaussian 
mixture given by, 



v k (m\X k ) = v k \k-i{m\X k ) 



i- Jk | k— 1 

l-P D ( m |X fe )+^ £ i;g^,m|X fe ) 

L z£2 t i=l 

(4.26) 
From the general PHD filter update equation 14.111 the components of the 
above equation are given by, 

v%(z,m\*k) = ^\A^k)M{m W %,P^) (4.27) 



P D {m\X k )w^ k _ iq ^{z,X k ) 

Jk\ k - 1 

(z) +Y>D(m\X k )w { g_ iq ^(z,X k ) 



I.' 

Ck' 



w k w x *) = — J— I — — ( 4 - 28 ) 



i=l 



where, q®(z,X k ) = Af(z;H k n^ k _ v S^). The components ^j and P® 
can be obtained from the standard EKF update equations, 

Sip =R k + VH k Pg_ x VHl (4.29) 

K^^P^Hl^r 1 (4-30) 

^l=^U+^-^04;U)) (4.31) 

PSi = [I'K^WH k ]P k %_ 1 (4.32) 

with Viffc being the Jacobian of the measurement equation with respect to 
the landmarks estimated location. As stated previously, the clutter RFS, C k , 
is assumed Poisson distributed [TH] , [33] in number and uniformly spaced over 
the mapping region. Therefore the clutter intensity is given by, 
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c fc (z) = X c VU(z) (4.33) 

where A c is the clutter density per scan, V is the volume of the surveillance 
region and and U(z) denotes a uniform distribution on the measurement 
space. 



4.5.2.1 The FBRM New Feature Proposal Strategy 

While any arbitrary strategy can be used for the new feature intensity 
bk(m\Zk-i,Xk-i), an intuitive strategy closely related to previous vector- 
based implementations is used in this work. As seen in eauation l4.241 given a 
GM intensity representation, the mean, covariance and weight of each Gaus- 
sian must be chosen. The GM component means and covariances determine 
the spatial distribution on the likely location of new features entering the 
map, with the sum of the weights, J2j=i w bl then providing an estimate of 
the expected number of new features to appear at time k. 

The new feature intensity at time fc, bk(jn\Zk-i, Xk-\), is adaptively de- 
termined by the previous measurements Zj»-i and the previous vehicle state 
Xk-i- The components of the GM of equation 14.241 are then determined ac- 
cording to 



^S = 0.01, $l = h-\4_ x ,x k _ x ), 

P$ = h'(^\X k ^)R[h(^\x k ^)] T 

where h~ l is the inverse measurement equation, R is the measurement noise 
covariance and h {fi^' , X k -i) is the Jacobian of the measurement model func- 
tion with respect to the Gaussian state, j. Therefore, the implementation 
initially considers all detections at time fc— 1 to be potential new features at 
time fc. This allows for features of low detection probability, which perhaps 
only become detectable at close ranges, to be reliably estimated. 



4.5.2.2 Gaussian Management and State Estimation 

Note from (j4.26[) that every measurement is combined with every Gaussian 
component, resulting in an explosive growth in the number of Gaussians in 
the posterior intensity. Therefore, as with previous GM implementations [38] . 
pruning and merging operations are required. Gaussians which arc determined 
sufficiently close (through a Mahalanobis distance threshold) are merged into 
a singular Gaussian as in [33J . Recall from equation l3.18l that the integral of the 
intensity function over a region on the space of features, 5, equals the number 
of features present in that region. Therefore, in some instances (particularly for 
closely lying features and/or high measurement noise), more than one feature 
can be represented by a single Gaussian component. A multi-level threshold 
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is thus used to estimate the dimensionality according to ex , with L being the 
number of features modelled by a single Gaussian component. State estimation 
occurs by extracting the means and covariances of the Gaussian components 
whose associated weights exceed the given thresholds. 

4.5.2.3 Pseudo-static Map, GMM PHD FBRM Pseudo Code 

This section details the pseudo-code of the GMM-PHD-FBRM filter. Table 
l4.6l outlincs the generation of the feature birth density whilst Table l4~71 details 
the construction of the predicted GMM PHD. Table |4~81 presents its update 
given the current measurement, Table 14.91 describes the Gaussian Manage- 
ment algorithm and finally, Table 14.101 shows the process used to extract 
feature map estimates at each time. 



Table 4.6 GMM-PHD-FBRM-Birth 



Algorithm GMM-PHD-FBRM-Birth (Z fe -i,X k i) 
// Implementation of section 14.5.2.11 (Any strategy is valid) 
// For each measurement 
1. for i = 1 to 3^4 do 
// initialise the mean 

2- ^ > /«S = /»- 1 (z^ 1 ,X fc _ 1 ) 
// initialise the covariance 

3- P$ = h\mi%,X k -i) 
II initialise the weight 

4- 41 = * 

5. end for 

// Set the number of birth components 

6- Jb,k = 3fe4 

// Construct birth PHD 

7. b k (m\Z k ^,X k ^) = {$ k ,PJ;%u>i%}£! 

8. return ( b k (m\Z k -i, -X"k-i) ) 



4.6 Algorithm Performance 

This section compares the proposed finite-set-based framework and GM im- 
plementation to popular solutions from the literature. Regardless of the choice 
of vehicle state representation, the vast majority of feature-based implemen- 
tations adopt an EKF framework coupled with data association and map 
management methods for propagating the feature map estimate [17], [14) . 
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Table 4.7 GMM-PHD-FBRM- Predict 



Algorithm GMM-PHD-FBRM-Predict(Z fe - X ,X k - 1; v k -i(m 


X fc _x)) 


//To obtain cauation (|4.25() 




// PHD Prediction: existing components 




1. for i = 1 to Jfc_i do 




// Static map assumption 




9 (*) (0 p(») p(») (0 (0 

z - Hfe|fc-1 — A*fc— l'-ntjfe— 1 — / fc-l>" ; fc|fc-l — w fe-l 




3. end for 




// Generate map birth components 




4. GMM-PHD-FBRM-Birth(Z fe _i, AVi) 




// PHD Prediction: new components 




5. for i = 1 to Jb.fe do 




// Components from birth proposal bk(m\Z k _i,X k ) 




°- A»fc| fe _i - Vb,k' r k\k-i ~ r b,k^k\k-i ~ w b,k 






7. end for 




// Total number of Gaussian components in v k \ k -i{m\X k _x) 




8- Jk\k-1 = Jk-l + ^&,fe 




// The predicted map PHD 




9.^| fe - 1 (m|X fe ) = { M «_ 1 ,P«_ 1 ,a;«_ 1 }£r i 




10. return(w fc | fc _ 1 (m|X fc )) 





PP> [TS], [T3], [IHj- As emphasised throughout this chapter, in their estima- 
tion of the feature map, estimates of the feature number and location are 
generated through independent filters. Under such methods, it is not clear 
whether Baycs optimality can be achieved, i.e. how is the Bayes risk de- 
fined and minimised. A simple nearest-neighbour EKF implementation is first 
adopted, coupled with the 'log-odds' map management method. The map- 
management approach 'propagates' the map size by assigning an intuitive log 
odds score to each associated or unassociated-associated feature. The prob- 
ability of a feature existing can then be readily recovered from the log-odds 
representation. For this analysis, a 95% x 2 association confidence window is 
chosen. This is identical to the map estimation method of FastSLAM (for a 
single particle) [T7], however in the analysis of this Chapter, the vehicle tra- 
jectory is assumed known and is not estimated. A joint compatibility branch 
and bound (JCBB) [33 data association approach, is also adopted, however 
given that the true location of the vehicle is assumed known, there is lack of 
measurement error correlation which JCBB exploits, thus limiting its useful- 
ness for mapping-only trials. As with the proposed framework, map size esti- 
mates are extracted by comparison with a predefined threshold, e. Tentative 
feature lists are maintained for unconfirmed features, which are then either 
added as confirmed features (with an increase in the map size estimate), or 
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Table 4.8 GMM-PHD-FBRM-Update 



Algorithm GMM-PHD-FBRM-Update (Z fc , X k , Ufc|fe_i(m|Jf fe _i)) 

// Initialise number of Gaussian components 

1. L = 

// Missed detections and Update Terms 

2. for i = 1 to Jfcu._i do 

// Increment component counter 

3. L = L+l 

// miss-detected component update of (|4.26[) 

*• ^k —rk\k~l' r k — r k\k-l ^_^ 

II miss-detected weight update of (|4.26|) 

s- 4 L) =(i-^)-i;Li 

// measurement prediction 

6 - 4i*-i = M^i|Li' Jf *) 

// Calculate Jacobian 

7. F = /i'( M gi_i' X ^ 
// Kalman Gain of ([430|l 

8. i?m=pW_ 1 [j?]'[5W]- 1 

// Innovation Covariance of (|4.29j) 
II Updated Covariance of (|4.32|) 

io. p« =[i-4 i) F]p«_ 1 

11. end for 

// For each measurement 

12. for i = 1 to 3^ do 

// For each map PHD component 

13. for j = 1 to J fe |fe_i do ^^ 
// Updated component mean of (|4.3ip 

is. " /^^L+^f-^L) 

// Updated GMM component covariance 

16- " Pi L+i) =P^l 

II Numerator of P~2"7jl 

17. T (J) = p DUJ g_ il2w sjfY - 5 



18. end for 



^((^-^Lr)^]- 1 ^-^-!)) 
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Table 4.8 (Continued) 



I J For each map PHD component 
19. for j = 1 to Jfc|fc_i do 
// denominator of (|4.27|) 

20. 4 i+j) =^)/( C (2)+£f=rT«) 

21. end for 

22. L = L+J fe | fe _i 

23. end for 

// Number of components in updated GMM 

24. J k = L 

// The updated map PHD 

25. Wfc (m|X fe ) = { M i i) ,Pi i) ,4 i) Kti 
26. return(X fc , v fc (m|X fc )) 



deleted. The purpose of the trials is not to indicate superiority over existing 
approaches, but to confirm the 'sensible' operation of this fundamentally new 
approach to propagating and evaluating feature-based map estimates. These 
results form the basis for future extensions to the SLAM problem, and to 
applications in cluttered environments, where the true qualities of the RFS 
framework will be demonstrated. 

Simulated trials are carried out based on the feature map shown previ- 
ously in Figure [2~5l For simplicity of presentation, the environment shall be 
restricted to comprise only point features, however the framework can read- 
ily be extended to other less structured environments with the use of robust 
feature extraction techniques. For each trial, all filters receive identical mea- 
surement sequences. The existence threshold is set at e^ = N— 0.4. Therefore, 
for previous approaches, ei = 0.6, since only one feature can be represented per 
Gaussian. For the proposed PHD solution, the Gaussian mass indicates the 
number of features, thus multi-feature thresholds, 62 = 1.6, 63 = 2.6, £4 = 3.6 
etc. are used. 

As outlined in [17) , the independent map management algorithms incorpo- 
rated into vector-based recursions also require score values to be intuitively 
set. To this end, an associated feature receives a score of +0.5, while an unas- 
sociated feature (within the sensor field of view), receives a score of —0.2. A 
simple existence counting rule is therefore established. While other methods 
of estimating the map dimensionality exist in the literature pQ, as empha- 
sised throughout this book, the independence of such approaches from the 
Bayesian update, compromises estimation optimally. The following FBRM 
error metric results are obtained from equation 14.61 with parameter c = 5, 
while the effects of adjusting this parameter are discussed in Section 14.6.51 
Results of applying the RFS map estimation framework in a real, outdoor 
environment will be shown in Section 14.6.71 
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Table 4.9 GMM-PHD-FBRM-Prune 



Algorithm GMM-PHD-FBRM-Prune (v, 


,(m\X k ) 


-L^miii; -*miii) <^maxj 


II 


Initialise number of Gaussian components 






1. 


L = 






// 


Remove components with weight below T m j n 




2. 


[ = {i = 1, . . . , Jk\u>k > ^min} // Gaussian 


merging 


• 


3. 


do while 1^0 






// 


Increment component counter 






4. 


L = L+l 






// 


Get index of max weight component 






5. 


(0 

j = arg max wjj. 






// 


Cluster those within distance Z? m i n 






6. 


K_={ie/|( M i i) -^ ) nPf]- 1 (Mi i) - 


fi k j) )<D min } 


// 


Combine component weights 






7. 








// Weighted average mean 






8. 








// 


Combined covariance 






9. 


p(i) _ 1 Y\,WfpM + ( T,( L ) „W 


U L) - 


/4°) T ) 


// 


Remove K from I and repeat 






10 


I = I- K 






11 


end for 






12 


■h = L 






// 


If max component number exceeded 






12 


11 J k ^_ Jniax 






13 


replace {Q k , pr k , P k } t t\ with those of the J max largest weights. 


11 


end for 






// 


The pruned map PHD 






13 


v k {m\X k ) = {u>t\~ti\P^}{U 






14 


return (v k {m\X k )) 







4-6.1 FBRM Error vs. Measurement Noise 

Increases in measurement noise (range/bearing), have the subsequent effect 
of increasing data association ambiguity in vector based methods, and hence 
the difficulty of the FBRM problem. To demonstrate the performance of 
the proposed method, trials are carried out in which the measurements are 
subjected to increasing amounts of noise. The measurement noise covariance 
for each trial is set at R= 7 [(0.25m) 2 0;0 (0.5°) 2 ], where 7 € [1,...,100]. 



4.6 Algorithm Performance 



()o 



Table 4.10 GMM-PHD-FBRM- Estimate 



Algorithm GMM-PHD-FBRM-Estimate(v fc (m|X fc ), T featu 
// Initialise the map estimate 

1. M k = 

2. for i = 1 to Jk do 
3- if w fc > Tf caturc 
// concatenate estimate 

4. M k = [M k nf] 

5. end if 

6. end for 

7. return (Mk) 



Figure 14.11 shows a comparison of the final FBRM error of the posterior 
feature map estimate for each filter at differing noise inflation values, 7. 



-EKF-FBRM 
■ JCBB-EKF-FBRM 
■GMM-PHD-FBRM 




Fig. 4.1 Comparison of FB mapping error vs. measurement noise for the proposed 
filters and classical vector EKF solutions. 



The result demonstrates the reasonable performance of the new framework 
for estimating the feature-based map. Note that for 7 < 20, all filters return 
comparable map estimates, indicating little data association ambiguity. As 7 
increases however, association errors are introduced into the NN and JCBB 
solutions resulting in the continual deterioration of the map estimation accu- 
racy. Note that the lack of measurement error correlation (since these feature 
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map estimation trials assume a known vehicle trajectory), reduces the effec- 
tiveness of JCBB. While the error in the GMM-PHD approach also increases, 
it appears robust to an increasing 7. 



4.6.2 FBRM Error vs. Clutter Rate 

Despite widespread acknowledgement that clutter is a prominent component 
of measurement uncertainty, a review of the literature indicates that algo- 
rithm performance versus increasing clutter rates is not often reported [39j, 
[13], |18| . In this chapter, the clutter density is defined as the density of 
clutter measurements within the sensor field of view. Furthermore, clutter 
measurements are uniformly distributed in polar space, i.e. the sensor mea- 
surement space. Setting the measurement noise multiplier at a fixed value of 
7 = 20, which from Figure |4~T1 is seen to be the point just prior to significant 
deviation in filter performances, the map estimation error for various clutter 
densities is analysed. 
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Fig. 4.2 Feature mapping error vs. clutter density for vector based NN-EKF and 
JCBB-EKF approaches and the proposed PHD framework. The proposed approach 
is seen to perform well in high clutter. 



Figure 14.21 plots the map estimation error for clutter densities ranging 
from A c = to A c = 0.0707m -2 , which correspond to a mean number of 
(Poisson distributed) clutter measurements of to 50 per 360° scan within 
a maximum range of 15m. Given the commonly adopted map management 
methods, it is unspecified how scoring regimes should be altered for a given 
clutter rate. Furthermore, feature existence and spatial estimates cannot be 
jointly propagated in current frameworks as outlined previously in Chaptcr[3] 
(Scction l3.2.ip . The proposed framework however, directly incorporates clut- 
ter probabilities into the filter recursion of equation 14.111 thus no parameter 
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adjustment/tuning is required. Although the error for each approach mono- 
tonically increases with clutter rate, it is evident that the proposed framework 
is robust and produces meaningful map estimates. Note again that the effec- 
tiveness of JCBB in these trials is limited by the lack of measurement error 
correlation. Comparison with JCBB is thus dropped for the remainder of the 
analysis. 



4-6.3 FBRM Error vs. Dynamic Object Density 



This section demonstrates algorithm robustness in the presence of dynamic 
objects, given its static feature assumption in the formulation. Dynamic ob- 
jects can corrupt the static map estimation process and, under this frame- 
work, are considered to be disturbances, which should not be declared as 
features. The measurement noise multiplier is again fixed at 7 = 20, with the 
clutter density set at A c = 0.014m~ 2 (10 clutter measurements per scan). Dy- 
namic features arc simulated to be uniformly distributed and evolve in time 
according to a Brownian motion model, Xk =Xk-i+uJk with uj^ ~ A/"(0, Q). 
The detection probability of a dynamic feature is set equal to that of a static 
feature (Pd = 0.95). Taking a map area to be 20m x 20to (as in Figure |2~5|) . 
the density of dynamic features is increased and the effects on the mapping 
accuracy examined. Figure 14.31 plots the posterior map estimation error at 
various densities of moving features. The results verify the merits of the pro- 
posed approach in that mapping accuracy does not drastically deteriorate in 
the presence of dynamic objects. 
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Fig. 4.3 Comparison of the map estimation error in the presence of increasing 
densities of moving features. 
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4-6-4 FBRM Error vs. Detection Probability 



The influence of feature detection probability (a joint function of the signal 
detection probability and the feature extraction algorithm) is investigated 
in this section. Measurement noise and clutter rates are set as in the dy- 
namic object trial of Section 14.6.31 while the feature detection probability 
is varied from 0.6 to 1. As is the case for the clutter trial of Section T4. 6. 21 
most map-management methods do not have a feature detection probability 
parameter [T3], [TB]. However, as shown in Figure FOl in some instances the 
results can in fact be superior, depending on the sequence of measurements 
supplied to the filter. As is evident from the update equation 14.111 given a 
single missed detection of feature j at time k, the updated weight becomes 
(1 — Pd)vj£ . Given two successive missed detections, the weight becomes 

(1 — Pd)(1 — Pd)w^ , which typically would be below the map dimensional- 
ity estimation threshold, e. Consequently, given successive missed detections 
prior to exiting the sensor field of view, features may not be correctly declared 
resulting in an increased mapping error. This problem becomes more evident 
as Pd decreases. This is in contrast to scoring regimes which depend on the 
total number of detections/missed detection of a given feature, as opposed 
to the order of the detection sequence. Enhancements to the proposed filter 
to improve this aspect are currently under investigation. 



0.6 
0.6 




Feature detection probability 



Fig. 4.4 Posterior map estimation error at increasing feature detection probabili- 
ties. 
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^.6.5 FBRM Error Metric Analysis 

Figure H~51 shows some simple generated input data, with the resulting poste- 
rior feature map estimates depicted in Figure |4~B1 A simple example is used 
to demonstrate the use of the map estimation metric. Visual inspection re- 
veals that, for the same set of measurements, a single false feature is declared 
with the NN-EKF approach coupled with increased feature localisation error 
when compared with the proposed method. For a quantitative comparison of 
the map estimate, as introduced in Section [4.31 the c parameter in equation 
14.61 gives the relative weighting of feature number and location estimation 
error. Further insight into the effects of a given choice of c is shown in Fig- 
ure 14.71 As is evident in the figure an increasing c parameter results in an 
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Fig. 4.5 A sample FBRM trial illustrating the ground truth (left) and raw mea- 
surement data (right). 



increasing overall error for the NN-EKF estimate. This is due to the con- 
tribution of the single false feature, which correspondingly has no effect on 
the error reported by GMM-PHD posterior estimate, since in this particular 
trial, it has correctly estimated the true number of features. The c parameter 
also determines the maximum distance at which an estimate is classified as 
a poorly localised feature estimate, as opposed to a false feature declaration, 
and should be chosen based on the maximum allowable estimated feature 
location error in a given application. 

For a given feature estimate-ground truth assignment, the value of p in 
equation 14.61 influences the contribution of the localisation estimation error. 
The visually evident improved feature location estimates of Figure 14.61 (right 
hand figure) are evident in Figure 14.71 by a lower total error reported for 
a given choice of p. To isolate the feature localisation estimation aspect, 
comparisons are also shown in which the false features from the NN-EKF 
estimate, was both ignored and included. 
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Fig. 4.6 Posterior FB map estimates from the classical NN-EKF-FBRM filter (left) 
and the proposed GMM-PHD-FBRM filter (right). Visual inspection indicates an 
improved map estimate from the proposed method, since all features are correctly 
declared (without false alarm) at 'closer' distances to the ground truth. 



NN-EKF 
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c,p = 2 

Fig. 4.7 FBRM error vs. c parameter, for a given value of p. The c parameter 
primarily determines the contribution of dimensionality errors to the total reported 



4-6.6 Computational Complexity Analysis 



At time k, given 3^ measurements and m^ map states, the computational com- 
plexity of a naive implementation of a NN-EKF FBRM solution is ©(jfctnfc), 
due to conditional feature / measurement independencies given an assumed 
known vehicle trajectory and evaluation of the measurement-map state as- 
sociations. According to update equation 14.111 the complexity of the pro- 
posed solution is also 0{ik^k)- Absolute computational load is compared 
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through side by side C-\ — \- implementations of varying map dimensionality, 
on Intel(R) duo-core 1.73GHz processors with 2GB RAM. Figure POl reports 
the average measurement update execution time, obtained through averaging 
1000 Monte Carlo updates, for an increasing map dimensionality. The figure 
clearly illustrates the expected linear increase in computation load for both 
approaches, with the proposed method requiring more processing time than 
a naive NN-EKF implementation, primarily due to the pruning and merging 
operations required for GMM implementations. Despite its increased load, 
real-time implementation is possible. 
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Fig. 4.8 Comparison of the computational complexity, demonstrating the linear 
increase in load with map dimensionality. 



4-6.7 Outdoor Experiment 



This section presents a practical implementation of the proposed finite-set- 
based framework for ground-based autonomous vehicles to demonstrate its 
applicability to real-world scenarios. The testing environment is a section of 
the Nanyang Technological University (NTU) university campus which con- 
tains numerous point features (trees, lamp posts, fire hydrants) which com- 
prise the point- feature map. Figure POl gives an overview of the testing area, 
with the inset vehicle-centric perspective showing the typical point features 
present. As with any practical implementation of an estimation algorithm, 
ground-truth is essential for error evaluation. For an FBRM implementation, 
both the true number and locations of all point features must be determined. 
This was achieved as best as possible, through observation of the synchronised 
video stream and successive manual scan matching of all the corresponding 



72 4 An RFS Theoretic for Bayesian Feature-Based Robotic Mapping 

laser data. The manually extracted centroid of all laser point features identi- 
fied on the video stream then provided the ground truth locationqj. The goal 
of an FBRM algorithm is therefore to estimate the number and location of all 
the point features within the mapped region (over a path of approximately 
300m). 




150 200 

Meters 

Fig. 4.9 An overview of the testing ground within the NTU university campus. 
The path (red) and raw laser measurements (yellow) are superimposed on a portion 
of a satellite image of the campus. The inset of a vehicle-centric view shows the 
environment mainly comprising point features such as trees and lamp posts. Laser 
data is projected into the image plane for verification. 



For clarity of presentation, the preceding simulated trials examined only 
the final posterior map estimates. However, taking the true map at time k, 
M-k-, to be the subset of the entire map which has entered the sensor field 
of view, the quality of the map estimate can be examined over the course 
of the trial. Feature number estimates are compared in Figure 14.101 at each 
time step with the true number also provided. It can be seen that feature 
number estimation error is introduced into the vector based approach as 
the feature initialisation and termination routines generally rely on accurate 
data association decisions. These can be prone to failure in environments of 
fluctuating detection probabilities and frequent spurious measurements. 



Note this does not account for the potential presence of any sensor bias. 
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Fig. 4.10 The posterior feature map dimensionality estimate following each mea- 
surement update compared to the ground truth. 



A graphical comparison of the final posterior map estimates is depicted in 
Figure B. Ill highlighting some mapping errors from both filters, ft is evident 
that the proposed approach reports fewer false features, but also missed some 
features of poor detectability and/or feature extraction reliability. Features of 
low detection probability, may be less consistently identified in the proposed 
method as outlined previously in Section 14.6.41 Figure 14.121 plots the FBRM 
joint estimation error after each update. The ideal error is the mapping error 
(as a function of the entire map), given that each feature that enters the 
sensor field of view is instantaneously detected and assigned an error-free 
localisation estimate. The proposed method closely follows the ideal error 
with temporary glitches as some dimensionality estimation errors occur (as 
seen in Figure H.10[) . The final posterior estimation error is also seen to be 
significantly less than that of the NN-EKF approach. 

It is remarked that this example serves to demonstrate that a first or- 
der approximation to the proposed framework generates sensible results to 
the feature map estimation problem. Better approximation and more efficient 
implementation requires further investigation as well as comparisons with nu- 
merous sophisticated data association and/or map management techniques. 
Such investigations will be left until Chapters [5] and [6] in which RFS SLAM 
estimation is demonstrated. 



4.7 Summary 

This chapter proposed a random set theoretic framework for feature based 
(FB) map estimation, in the context of autonomous navigation problems. 
Estimating a FB map encompasses estimating the location of an unknown 
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Fig. 4.11 Final posterior feature map estimate comparison. The proposed method 
demonstrates improved feature mapping capabilities, however some false features 
are evident where container corners, were misinterpreted by the clustering algorithm 
as being point features. 



NN-EKF estimation error 
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Fig. 4.12 FBRM estimation error at each measurement update in comparison to 
the total error given perfect detection and estimation once a feature enters the 
sensor's field of view. 
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number of features, however most current solutions do not jointly estimate 
the number of features and their locations. This is necessary as the classi- 
cal vector-based approach cannot jointly encapsulate spatial and existence 
uncertainty. By adopting a random finite set map and measurement, the un- 
certainty in the map size and feature locations can be jointly propagated and 
estimated. This new approach therefore generalises the notion of Baycs opti- 
mality to the realistic situation of an a priori unknown number of features. 
Furthermore, the new framework allows for a mathematically consistent error 
metric to be readily adopted for the joint evaluation of FB map estimation 
error. 

To demonstrate the applicability of the proposed framework to solve real 
FB autonomous problems, it was shown how the first order approximation, 
the PHD filter, could be implemented. Through simulated and real experi- 
mental trials, a proof-of-concept was presented. The proposed filter alleviates 
the need for data association and map dimensionality estimation filters, as the 
proposed theory incorporates these sources of uncertainty into the Bayesian 
recursion. Given its non-reliance on data association, the proposed approach 
may be more suited to applications with high clutter and/or a highly ma- 
noeuvring vehicle. 



4.8 Bibliographical Remarks 

Whilst most RM algorithms are occupancy-grid based, feature-based RM al- 
gorithms are typically adopted from feature-based SLAM algorithms, which 
have been modified to incorporate a known vehicle location. That is, exami- 
nation of the posterior map estimate from a feature-based SLAM algorithm 
where the vehicle location uncertainty is zero. This has the effect of removing 
the cross-correlations between landmarks in the map and the vehicle state, a 
property which is also exploited in Rao-Blackwcllised FastSLAM implemen- 
tations [17j . 

The most common types of exteroceptive sensors deployed for FBRM 
applications are 2D range-bearing measuring devices. Such sensors' measure- 
ments are subject to often assumed Gaussian range and bearing measure- 
ment noise, which are typically used to update the time predicted map state 
through an EKF filtering framework JTH] [15]. Due to the non-linearity of 
the measurement equation, Taylor approximations of the 'extended' filter are 
required. 

In general, the FB autonomous framework is closely related to the 
multi-sensor, multi-target filtering problem, where the objective is to jointly 
estimate the time-varying number of targets and their states from sensor 
measurements in the presence of data association uncertainty, detection un- 
certainty, clutter and noise. The first systematic treatment of this problem 
using random set theory was conceived by Mahler in 1994 [5D], which later 
developed into finite set statistics (FISST). Moreover, this treatment was 
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developed as part of a unified framework for data fusion using random set 
theory. [2JJ provides an excellent overview of FISST. 

Mahler refined FISST to what he called generalised FISST and published 
this along with the derivation of the Probability Hypothesis Density (PHD) 
filter in 2003 [2,. Additionally, the relationship between FISST set deriva- 
tives and probability density (as Radon Nikodym derivatives of probability 
measures) for random finite sets was established in |5j. Further, generic se- 
quential Monte Carlo (SMC) implementations of the multi-object Bayes filter 
and PHD filter, with convergence analysis was also provided. Numerous in- 
dependent SMC solutions were also proposed [2], [22], [23], [22] and applied 
to a wide range of practical problems including feature point tracking in im- 
age sequences [25] , tracking acoustic sources from Time Difference Of Arrival 
(TDOA) measurements [2S] > and tracking using bi-static radar data [2Z] . The 
reader is referred to |48j for a more complete survey of applications. 

As indicated in Chapter[3j the FISST Bayes multi-object filter is generally 
intractable. Therefore, in 2000, Mahler proposed to approximate the multi- 
object Bayes recursion by propagating the Probability Hypothesis Density 
(PHD) of the posterior multi-object state [2], [3J. 

In 2005 a closed-form solution to the PHD recursion for the linear Gaussian 
multi-target model was published together with the Gaussian Mixture (GM) 
PHD filter for linear and mildly non-linear multi-target models [33J. While 
more restrictive than SMC approaches, Gaussian mixture implementations 
are much more efficient. Moreover, they obviate the need for clustering - 
an expensive step in the SMC implementation. Convergence results for the 
GM-PHD filter were established in [49 . Further extensions were introduced 
in 2006 through the cardinalised PHD (CPHD) recursion - a generalisation 
of the PHD recursion that jointly propagates the posterior PHD and the 
posterior distribution of the number of targets [SU] , [5TJ . A detailed treatment 
can be found in Mahler's recent book [3J. 

Since FB map estimation involves the estimation of a set of features us- 
ing noisy, and cluttered measurements, it is evident that the mathematical 
foundation of multi-object tracking is related to the FBRM framework and 
thus provides motivation to re-examine the rationale and map estimation op- 
timality of existing approaches. The following section therefore examines the 
underlying estimation theory fundamental to the problem of estimating the 
FB map. 



Part III 

Random Finite Set Based 

Simultaneous Localisation and Map 

Building 



Chapter 5 

An RFS 'Brute Force' Formulation for 

Bayesian SLAM 



5.1 Introduction 

The feature-based (FB) SLAM scenario is a vehicle moving through an envi- 
ronment represented by an unknown number of features. The classical prob- 
lem definition is one of "a state estimation problem involving a variable num- 
ber of dimensions" |28j . The SLAM problem requires a robot to navigate 
in an unknown environment and use its suite of on board sensors to both 
construct a map and localise itself within that map without the use of any 
a priori information. Often, in the planar navigation context, a vehicle is 
assumed to acquire measurements of its surrounding environment using on 
board range-bearing measuring sensors. This requires joint estimates of the 
three dimensional robot pose (Cartesian x and y coordinates, as well as the 
heading angle 9), the number of features in the map as well as their two di- 
mensional Euclidean coordinates. For a real world application, this should be 
performed incrementally as the robot manoeuvres about the environment. 
As the robot motion introduces error, coupled with a feature sensing er- 
ror, both localisation and mapping must be performed simultaneously [5]. 
As mentioned in Chapter [2j for any given sensor, an FB decision is subject 
to detection and data association uncertainty, spurious measurements and 
measurement noise, as well as bias. 

The majority of proposed algorithms, stemming from the seminal work 
of [5], adopt an augmented state containing both the vehicle pose estimate 
and the estimate of the map. It is important to note however, that the exam- 
ple discussed in [8] consisted of a map containing features of unity detection 
probability, assumed the measurement-feature association was known, and 
that the sensor reported no spurious measurements. With these strict as- 
sumptions, the Kalman based SLAM estimate is indeed Bayes optimal. This 
work was incorporated into multiple Kalman-based solutions to the SLAM 
problem |52) . 
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The presence of detection uncertainty and spurious measurements have 
also been long acknowledged in the SLAM community, and subsequently 
feature initialisation and termination algorithms are frequently incorporated 
into the vector-based SLAM algorithm [52] (also shown in figure fTTTj) . This 
chapter again emphasises that these are required due to the inability of a vec- 
tor representation to incorporate uncertainty in the number of dimensions, 
and highlights that such methods (which are independent of the filter recur- 
sion) compromise filter performance. As shown in this chapter, this can result 
in filter divergence and large mapping error, especially in scenarios of high 
clutter and large data association ambiguity. Through the re-formulation of 
the classical SLAM problem, and by explicitly incorporating the problem of a 
variable number of dimensions into the filter recursion, increased robustness 
in noisy scenarios will be demonstrated. 

A finite set-valued measurement allows for the inclusion of spurious mea- 
surements directly into the measurement equation, as introduced in equation 
12.101 which is then the union of the set-state dependant measurements (as is 
the case in the classical Bayesian SLAM formulations) and the set of spurious 
measurements. A finite set-valued map state can be constructed from the set 
union of the existing features and the new features which may appear in the 
map due to the motion of the robot, as introduced in equation 13.211 

This chapter subsequently casts the SLAM problem into a random set the- 
oretic filtering problem that incorporates the joint estimation of the vehicle 
pose, feature number and corresponding feature locations. The term "Brute 
force" is used to describe the concept presented here, since each estimated 
feature is augmented with a hypothesised vehicle trajectory. While this is the- 
oretically sound, and simple to implement, its computational burden becomes 
obvious when one considers that a single PHD is propagated, which requires 
many Gaussian functions to represent a single feature, since that feature 
can be augmented with many hypothesised trajectories. Although computa- 
tionally intractable in any realistic environment with significant numbers of 
features, its implementation is included here to demonstrate a viable, and 
theoretically simple, RFS based SLAM solution. A more elegant, and com- 
putationally tractable solution, based on Rao-Blackwellisation, will be the 
subject of Chapter [51 

This chapter is organised as follows. A recap of the RFS Bayesian, SLAM 
formulation is given in Section 15.21 based on the RFS map transition func- 
tion introduced in Chapter [3] and the RFS measurement model, introduced 
in Chapter [2J Section [5 . 31 introduces an augmented joint vehicle-map RFS to 
incorporate vehicle location uncertainty. The PHD of the augmented state 
recursion is then presented, and the PHD-SLAM filter is introduced. Using 
Gaussian noise assumptions, an extended-Kalman Gaussian Mixture (GM) 
implementation is developed in Section 15.41 This implementation accounts 
for the non-linearity in the measurement equation and jointly estimates 
the feature number in the map, their corresponding states and the vehi- 
cle pose. Importantly, this can be achieved without the need for explicit data 
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association decisions and/or feature management algorithms. Simulated map- 
ping and pose estimation results are shown in Section T5 . 61 where the proposed 
GM-PHD SLAM filter is tested on simulated data which contains a large 
number of spurious measurements. Results show the efficacy of the proposed 
framework for solving the Bayesian SLAM problem. Real RFS based SLAM 
results in both outdoor and coastal environments (at sea) will be given for 
the formulation presented in chapter [BJ 



5.2 RFS Formulation of the Bayesian SLAM Problem 

The starting point for the RFS, SLAM formulation is a recap of the map 
transition equation for RFSs, introduced in Chapter [2] (equation I2.10p and 
the RFS measurement model introduced in Chapter [3] (equation I3.21|) . 

To incorporate the fact that new features enter the map, A4 k , with time, 
let the map state M. k be an RFS which evolves in time according to, 

M k = M k -i U B k (5.1) 

comprising the set union of the previous RFS multi- feature map, M. k -i and 
the RFS of the new features at time k, B k . These sets are assumed mutually 
independent. 

As in equation l2.10l (repeated below for convenience), to contend with the 
realistic situation of missed detections and clutter, the measurement is also 
modelled as an RFS. Given the vehicle state, X k , and the map Ai k , the 
measurement consists of a set union, 

Z k = (J V k (m,X k )UC k (X k ) (5.2) 

meMk 

where T> k {m,X k ) is the RFS of the measurement generated by a feature at 
?n and C k (X k ) is the RFS of the spurious measurements at time k. 
For each feature, to G M k , and z e Z kl 

■D k (m,X k ) = {z} (5.3) 

with probability density Pd(to, X k )g{z\m, X k ) and T> k (X k , to) =0 with prob- 
ability 1 — P]j(m,X k ), where PD(m mfc , X k ) is the probability of the sensor 
detecting the rtlj, feature from pose X k . Using Finite Set Statistics [3], the 
probability density that the sensor produces the measurement set Z k given 
the vehicle state X k and map M. k at time k is then given by [2]: 

g k (Z k \M k ,X k ) = J2 9v(W\M k ,X k )g c (Z k -W) (5.4) 

wc.z fc 
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with gj)(W\Mk 7 Xk) denoting the density of the RFS of observations, 
T> k (m,X k ), generated from the features in the observed map M k given the 
state of the vehicle, and gc(Z k — W) denoting the density of the clutter RFS, 
Cfc- gv(W\Mk,Xk) describes the likelihood of receiving a measurement from 
the elements of the set-valued map which incorporates detection uncertainty 
and measurement noises. gc(Z k ~W) models the spurious measurement rate 
of the sensor and is typically a priori assigned [15] |14j . Expanding the multi- 
target RFS Baycs recursion of [3] to include the vehicle state, the optimal 
Bayesian SLAM filter then jointly propagates the set of features and the 
vehicle location according to, 



Pk(X : k ,M k \Z 0:k , Uo-.k-l, A"o) — 

gk(Zk\Mk, Xk)pk\k-i(Xo;k, Mk\Z :k-ii U :k-i,X ) 
J J gk(Z k \Mk,X k )p klk _ 1 (X 0:k ,M k \Zo:k-i,Uo:k-i,Xo)dX k ij,(dMk) 

where, 



(5.5) 



Pk\k-l{Xo-.k, -Mfc|Z :fc-l, Uo : k-l,X ) = 

fx{X k \X k -i,Uk-i)pk-i(XQ :k -i,Mk-i\Z a .. k ^ 1 ,U a ..k-2,X Q )dX k ^ 1 (5.6) 



and /i is a reference measure on the space of features. As noted in Chapter 
[H in a direct implementation of the vector-based Bayesian SLAM recursion 
of eauation l2.15l computational complexities and multiple integrals generally 
lead to intractable solutions. Therefore the PHD approximation introduced 
in Chapter [3] is used. 



5.3 The 'Brute Force' PHD SLAM Filter 

The RFS formulation of the general Bayesian SLAM problem was described 
in the previous section. This section modifies the formulation to admit a 
compact PHD solution. The key to the approach is the introduction of a new 
RFS state, y, which comprises the unordered set of n elements, (, such that 
at time fc, 

y k = Kiel ■■■,&} (5.7) 

where each £ k comprises a map state, m k , conditioned on a vehicle trajectory, 
Xo :k . rife will be defined below. Conditioning each feature state, m, on the 
history of vehicle poses X^. kl introduces a conditional independence between 
feature measurements allowing the joint states, C, k to be independently prop- 
agated through the PHD SLAM framework [17j . Following the introduction 
of the PHD SLAM filter in Section EH a "Brute force" PHD SLAM filter 
can then be derived if, in equations 13.231 and 13.241 

A — > Cfe- (5.8) 
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Each element of y k , evolves in time according to the transition 
f(Ck\(k-i,Uk-i) and, if the feature in Cfc is detected by the sensor at 
the conditioning pose X k , a measurement z is generated with likelihood 
PD{Ck)9k{z\Ck)- The precise reason why the conditioning of the feature state 
m, with each of the N hypothesised vehicle trajectories to form the state (k, 
is permitted under the RFS framework is due to Campbell's theorem, as ex- 
plained in Appendix^ Therefore, let the vehicle state be sampled by N par- 
ticles, to produce Nxm k = ttfc augmented states, Cfc- Given a set of augmented 
features, (k, joint estimates of the number of features, their locations, as well 
as the vehicle state, can then be obtained. Hence this SLAM implementation 
estimates a potentially extremely large, single PHD (intensity function) con- 
taining representations of each feature, conditioned on each vehicle trajectory. 
Referring to the GM PHDs of figures O and GEH in the "Brute force" SLAM 
case, multiple Gaussians are necessary to represent each single feature, since 
each must be conditioned on every one of the N hypothesised trajectories. 
In contrast to the FBRM case of Chapter 21 computationally, a copy of each 
hypothesised vehicle trajectory (rather than the single known vehicle pose) 
is necessary to condition each feature. The PHD-SLAM recursion can then 
be formulated in terms of the state elements ( k . The prediction of the state 
intensity Vk\k-i(Ck) is then given by 



Wfclfc-i(Cfc) = : / f(Ck\Ck-uUk-i)vk-i(Ck-i)dCk-i+b k (Ck\X k ) (5.9) 

f(Ck\Xk-i,mk,Uk-i)vk-i(Xk-i 7 m k )dXk-i + b k (( k \X k ) 



where /(CfelCfe-ij^fc-i) incorporates both the assumed vehicle and feature 
predicted transition functions. As before, bk(Ck\Xk) models the new features 
entering the vehicles FoV. The corrector equation for the combined state, 
SLAM intensity function is 



Vk(Ck) =v k \ k -i(Ck) 



where again at time k, 



l-P D (Ck) (5.10) 

\p Pp(Ck)9k(z\Ck) " 

+ ^ c k (z\X k ) + J P D (09k(z\0vk\k-i(0<^ 



bk((k\Xk)= intensity of the new feature RFS B k , 
9k{z\Ck) = likelihood of z, given the joint state £&, 
Pd (Cfe ) = probability of detection of the feature in 

Ck, given the pose in ( k , 
Ck{z\Xk) — intensity of the clutter RFS C k (X k ). 
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In [53], Gaussian noise assumptions were used to obtain closed form solu- 
tions for the target tracking PHD filter. Similarly for the PHD-SLAM filter, 
GM techniques can be applied to solve the PHD-SLAM joint intensity recur- 
sion of equation 15.111 It is also possible to use a particle-based approach [4j , 
however, for mildly non-linear problems the Gaussian mixture approach is 
much more efficient. The following section thus presents a GM implemen- 
tation of the PHD-SLAM filter, while a particle based version is left until 
Chapter El 



5.4 Gaussian Mixture (GM) PHD-SLAM 

Let the joint intensity, Vk-i(Ck-i), at time k— 1 be a Gaussian mixture of the 
form, 

^.-i(a-i)= M4i^-* ( 5 - n ) 

composed of N x Jk-i Gaussians, with wA_i, Wl, and -F^J^ being their 
corresponding weights, means and covariances respectively. Note that the 
weight, uijZ-L is a weight on both a particular feature state, m, and a particular 

vehicle pose Xj?j 1 , i-e. on the joint state Q-i- 

Since the map is assumed static, the joint state transition density is 

f x (xi n) \xi% Uk-xMmk-mk-i) (5.12) 

where Xj c _ 1 is one of N vehicle pose particles at time k— 1 and 5(rrik— TOfc-i) 
is a Dirac delta function to mathematically incorporate the fact that the map 
must remain static in this case. Let the new feature intensity at time k also 
be a Gaussian mixture, 

NxJ b ,k 

where Wi k , /Xt l { and P^l determine the shape of the new feature GM proposal 
density according to a chosen strategy. This is analogous to the proposal 
distribution in the particle filter [17j and provides an initial estimate of the 
new features entering the map (see Section 15. 4. 1[) . Again, each new feature 
density component is conditioned on each predicted vehicle pose particle, 

(n) 

Xf, to form the NxJb,k components of the GM new feature intensity. That 
is, for each hypothesised vehicle trajectory, a set of Jb,k Gaussian components 
are initialised. Recalling that each Gaussian models the feature and vehicle 
trajectory, Ji>j~ copies of a given trajectory are distributed to Jf, ^ features. 
This is in contrast to classical Rao-Blackwellised approaches, as presented in 
the following chapter, where only a single copy of each trajectory is required. 
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However, in the PHD-SLAM filter, this conditioning allows for each joint 
state (feature and trajectory) to be independently estimated via the PHD 
framework. Therefore, the predicted intensity, Vk\k-i((k) is also a Gaussian 
mixture 

f*i*-i(&) = E v% k -Mbi*k\k-i> p !&-i) ( 5 - 14 ) 

where, J k \k-i = N(J b , k + J fc _i) and, 

W k[k-1 = W k~l 

Mfeife-i = /"fcifc— i ^for i e {1, . . . , Nx Jfc-i} (previously observed features) 

p(i) _ p« 

^fe|fe-l ~~ ^fc-l 

/^lii-i = A*& fc ^ or i *= {AxJfc_i + !,•■•, NxJt^} (newly observed features). 
p(») _ p(») 

Assuming a Gaussian measurement likelihood, g(z\(^k), analysis of equation 
15.111 shows that the joint posterior intensity, Wfc(Cfc), is consequently also a 
Gaussian mixture, 



Vk(Ck) = Ufc|fc-l(Ck) 

where, 



■h,- 



k—i 



i-p D (c k )+Yl E <$*(*&) 



zez k j=i 



(5.15) 



,W /,IA\ _„„(*) A/TV. „(*) p(') 



, fc WCfc) = <W(C;^r*.- p *i*) ( 5 - 16 ) 

^ = — (5 - 17) 

Cfc (z)+ e ^(a^'L^v^Cfc) 

with, 9^(2, Cfc) =N(z] ^feMfeife-i) 5*1 )• The component distributions are 
represented by their first and second moments obtained from the standard 
EKF update equations, 

Si i] =R k + VH k Pg_ x SIHl (5.18) 

*» = PjjLVflZ-pW]- 1 (5.19) 
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pg = [I-K^H k ]Pg_ x (5.21) 

with ViJfc being the Jacobian of the measurement equation with respect to 
the features estimated location. As stated previously, the clutter RFS, C k , is 
assumed Poisson distributed |18) . |14j in number and uniformly spaced over 
the sensor surveillance region. Therefore the clutter intensity is, 

c k (z) = X c VU(z) (5.22) 

where A c is the average number of clutter returns, V is the volume of the 
sensor's surveillance region and 1l(z) denotes a uniform distribution over 
range and bearing. As stated in Section [5. 11 the Gaussian number growth of 
this formulation becomes very large and hence Gaussian pruning and merging 
methods arc used as in 1331. 



5.4-1 The SLAM New Feature Proposal Strategy 

The new feature proposal density, equation 15.131 is similar to the proposal 
function used in particle filters, and is used to give some a priori information 
to the filter about where features are likely to appear in the map. In SLAM, 
with no a priori information, b k , may be uniformly distributed in a non- 
informative manner about the space of features (analogous to the prior map 
used in occupancy grid algorithms). However, in this work the feature birth 
proposal at time k is chosen to be the set of measurements at time k— 1, Z k -\- 

The sum J2i=*i b ' k w bi ^ nen gi yes an estimate of the expected number of new 
features to appear at time k. The components of the Gaussian mixture used 
to form bk are determined in exactly the same way as for the FBRM case, 
described in Section ft. 5 .2. II 



5.5 Brute Force SLAM Pseudo-code 

This section details the pseudo-code of the proposed PHD-SLAM algorithm. 
Tabic 15.11 presents the birth proposal algorithm which accommodates new 
features entering the map as well as aiding particle diversity since each new 
potential feature is seeded with a corresponding potential vehicle pose. Table 
15.21 outlines in the prediction module while Tables 15.31 and 15.31 describe the 
update module. Vehicle pose and map estimation are achieved via the process 
detailed in Table El 



5.6 Algorithm Performance 87 

Table 5.1 PHD-SLAM-Birth 



Algorithm PHD-SLAM-Birth(Z fc _ 1 , v k -i(t\Zk-i)) 

II Generate ean. (|5.13p . Note: Any arbitrary strategy is valid. 
// For each measurement 

1. for i = 1 to 3/^ do 
// For each particle 

2. for j = 1 to Jk-i do 
// initialise the feature mean 

3. ^ ^ = / l - i (z« 1 ,4 J 2 1 ) 

// initialise the concatenated state mean 

4 - ^b,k ^ i A k-l V b,k\ 

II initialise the covariance 

_ ((i-l) X J h _ 1+ j) _ , (j) . 

o. r bJt - ti (Vb,k,^k-i) 

II initialise the weight 

6. 4%~ 1)xJk - 1+J) = a 

7. end for 

8. end for 

// Set the number of birth components 

9- Jb,k = iki x Jk-l 

II Construct birth PHD 

11. return ( b k (C\Zk-i) ) 



5.6 Algorithm Performance 

This section analyses the performance of the proposed GM-PHD SLAM filter 
in a simulated environment, and compares it to a FastSLAM implementation 
using maximum likelihood data association decisions and Log-Odds feature 
management [17] . The vehicle is assumed to be travelling at Sms^ 1 while 
subject to velocity and steering input noises of lms -1 and 5° respectively. 
Only 10 particle samples arc used for both filters and both filters receive 
the same noisy input samples and sensor measurements. Two simulated com- 
parisons are performed in an 'Easy' and 'Difficult scenario'. For the 'easy' 
scenario, the clutter parameter, A c = m -2 , feature detection probability is 
0.95, and the measurement noises are 0.25m in range and 0.5° in bearing. 
For the 'difficult' scenario, A c = 10 m~ 2 (i.e. 10 false alarms occur for every 
square metre of area within the sensors FoV) , feature detection probability is 
again 0.95 and the measurements noises are set at 12.5m in range and 25° in 
bearing. The effect of the artificially large measurement noises are to give the 
appearance of closely spaced features, hampering data association decisions 
and feature map building. 
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Table 5.2 PHD-SLAM-Predict 



Algorithm PHD-SLAM-Predict (v k -i 


(C|Z*_i),17fc-i) 


II 


Generate ean.(|5.14|) 




II 


Generate map birth components 




1. 


GMM-PHD-FBRM-Birth(Z fc _i, w fc _i(C 


2*_i)) 


// 


append PHD with birth components 




2. 


br i = 1 to Jb.fc do 




3. 


, .M.-1-H) 1 , Xi) 




4. 


„(Jfc-i+») ,,(») 




A*fc_i = MU 




5. 


p(./ fc _i+i) _ p (i) 




6. 


end for 




// 


increment component counter 




7. 


Jk\k-1 ~ Jk-l + Jb,k 




8. 


for i = 1 to Jfe| fe _i 




// 


sample a pose from the vehicle model 




9. 


^iu-M^Wt^-i) 




// 


static map assumption 




10 


(*) (0 
v k\k-l ~ v k-l 




11 


„(*) _ ry(') ,,(*) i 

^k\k-l ~ l-^felfe-l ^fc|fc-lJ 




12 


p(») _ pM 
■ r fe|fe-l — ^fc-l 




13 


W (0 




14 


end for 




// 


The predicted SLAM PHD 




15 


Ufe|fe_l(C|^fc-l) = {^k\k-V P k\k-V U k\k 


1 «/fe j fc — 1 

-lJi=l 


16 


return(u fe | fe _ 1 (C|Z fc _i)) 





Figure 15.11 shows the estimated vehicle trajectory and corresponding fea- 
ture map from both filters, in the case of the 'Easy scenario'. 

Both results compare well with ground truth (green). This result veri- 
fies the accuracy of the proposed PHD-SLAM filter, in its ability to jointly 
estimate the vehicle trajectory, the number of features, and their correspond- 
ing location, without the need for external data association and feature map 
management methods, as are required by FastSLAM (and other vector-based 
solutions). 

The missed feature declaration highlights an issue of the proposed method 
with respect to Pd(0c)- In the presented implementation, this is simply a 
binary function which has an assumed value of 0.95 if the feature is predicted 
to be within the sensor FoV, and if it is not. Vehicle and feature estima- 
tion uncertainty may result in a feature erroneously being hypothesised of 
being within the FoV, or vice-versa. If the proposed filter then receives a 
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Table 5.3 PHD-SLAM-Update 



Algorithm PHD-SLAM-Update (Z fe , v k \k-i(QZk-i)) 
II Initialise number of Gaussian components 

1. L = 

// Missed detections and Update Terms 

2. for i = 1 to Jfc|fc_i do 

// Increment component counter 

3. L = L+l 

// Updated feature equals predicted feature 

// Updated joint mean 

// weight decreased 

// measurement prediction 

7- z k\k-\ = M^fcifc-i'^fcifc-i) 

3// Calculate Jacobian 

// Innovation Covariance of Q5.18|) 

II Kalman Gain of (|5~T9|l 

10. Kf=P%_ x \EY[sf\^ 

II Updated Covariance of (|5.21[) 

12. end for 

// For each measurement 

13. for i = 1 to 3^ do 
// For each component 

14. for j = 1 to Jfc|fc_i do 

// Updated map component mean 

// Updated joint mean 

1fi ,,( i +j)_r,. y(») 1 

lb. Hk -K A fe|fe-iJ 

// Updated GMM component covariance 

i 7 p(L+j) _ p(j) 

k 7~ u : k 
II Numerator of ([5H|) 

18. r« = P^Li|2^]-°- 5 

xexp((4 i )-^)_ i)[5 0-) ] - 1( 4i)_^)_ i)) 

19. end for 
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Table 5.3 (Continued) 



I J For each map PHD component 
20. for j = 1 to Jfc|fc_i do 

// denominator of (|5.16p 

2i. 4 L+3) = T^/(c( Z )+zt l r^) 

22. end for 

23. L = L+J k \ k _ 1 

24. end for 

// Number of components in updated GMM 

25. J k = L 

// The updated map PHD 

26.v k (C\Z k ) = {rf\p£\u,P}£ 1 
27. rcturn( Vfc (C|Z fc )) 



Table 5.4 PHD-SLAM-Estimate 



Algorithm PHD-SLAM-Estimate(u fc (C|Z fc ), T feat , 
// Initialise the map estimate 

1. M k = 

2. Q k = 

3. for i = 1 to J k do 

4. n k = n k + 4 l) 

5. if 0J k > r fcaturo 
// concatenate estimate 

6. M k = [M k 4 ] ] 

7. end if 

8. end for 

// expected pose 

I— 1 

10. return (X fc ,7V4fc) 



measurement contrary to the prediction, the resulting feature weight may 
be detrimentally reduced, and a missed feature declaration may occur. The 
uncertainty in the estimated sensor FoV is not considered in this implemen- 
tation. 

The raw measurements as well as the final posterior joint estimate of both 
filters for the 'hard' scenario are presented respectively in figures 15.21 and 
15.31 As can be seen, the FastSLAM filter shows divergence of the estimated 
vehicle trajectory from its true value, as well as many falsely estimated map 
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features. The proposed filter however, displays dramatically reduced feature- 
based mapping error in the face of large data association uncertainty and 
large quantities of spurious measurements, reporting only a single false fea- 
ture and a single missed feature over the entire run. This is expected, as the 
clutter rate is integrated directly into the filter recursion in an optimal man- 
ner and feature management is performed jointly with feature and vehicle 
location estimation. The key to this vast difference in performance in clut- 
tered environments, is evident in figure 15.41 The figure shows the estimated 
number of features in the map over time, for both the discussed filters, as 
well as the number of false measurements at each time instant. The proposed 
filter accurately tracks the true number of features over time, whereas the 
FastSLAM filter deviates drastically in the face of the challenging spurious 
measurements and data association ambiguities. Estimating the true number 
of state dimensions influences the accuracy of the overall SLAM filter. The es- 
timated vehicle trajectory also displays less error than that of the FastSLAM 
approach. Similarly to FastSLAM, increased trajectory estimation accuracy 
may be possible by increasing the number of pose samples. Figure [375] com- 
pares the estimated vehicle heading over the course of the test, highlighting 
the increased accuracy of the proposed filter. 
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Fig. 5.1 Comparative results for the proposed GM-PHD SLAM filter (black) and 
that of FastSLAM (red), compared to ground truth (green). 
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Fig. 5.2 The predicted vehicle trajectory (blue) along with the raw sensor measure- 
ments for the 'hard' scenario, at a clutter density of 0.03m -2 . Also superimposed 
are the ground truth trajectory and feature map (black crosses). 
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Fig. 5.3 The estimated trajectories of the GM-PHD SLAM filter (black) and that 
of FastSLAM (red). Estimated feature locations (crosses) are also shown with the 
true features (green circles) 
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Fig. 5.4 The estimated feature number for the proposed (black) and FastSLAM 
(red) filters. The green curve shows the ideal estimate, which grows as more features 
enter the FoV of the sensor, during vehicle motion. The blue curve shows the 
randomly generated number of clutter measurements. 
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Fig. 5.5 The error in vehicle heading estimate for the proposed (black) and Fast- 
SLAM (blue) filters. 
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5.6.1 A Note on Computational Complexity 



As is evident from the update of eauation l5.15l the proposed algorithm scales 
linearly with 0(NJ k \ k _ 1 $k), which equals that of a naive FastSLAM imple- 
mentation. Future work will address reducing this to a Log order complexity 
of the number predicted map states JW_i. The presented results illustrate 
the effectiveness of the new finite-set based SLAM framework, and the pro- 
posed GM-PHD implementation, when compared to vector-based solutions 
which fail to jointly consider the entire system uncertainty. 



5.7 Summary 

This chapter outlined a 'Brute force' formulation of the Bayesian SLAM prob- 
lem, using random set theory. The set theoretic approach allows for detection 
uncertainty, spurious measurements as well as data association uncertainty 
to be incorporated directly into the filter recursion. This is in contrast to 
vector-based SLAM which requires additional algorithms and pre/post pro- 
cessing to solve the data association problem prior to filter update, and to 
extract estimates of the number of features present in the map. These are 
necessary as such sources of uncertainty are not considered in the classical 
vector-based measurement model and subsequent filter recursion. Previous 
Bayesian SLAM solutions also lack a concept of Bayesian optimality as the 
variable dimensionality problem is not jointly considered. 

Propagating the first order statistic of the random set (the probability hy- 
pothesis density) is a common method of reducing the computational require- 
ments of implementing the set- valued Bayesian recursion. By augmenting the 
feature state with a history of vehicle poses, conditional independencies be- 
tween the features and the vehicle state are introduced. The joint vehicle 
feature RFS was shown to maintain the necessary Poisson assumptions for 
application of the tracking based PHD recursion for the PHD-SLAM prob- 
lem. A Gaussian mixture implementation of the PHD-SLAM filter was out- 
lined assuming a Gaussian system with non-linear measurement and process 
models. The proposed finite-set filter was compared to a FastSLAM imple- 
mentation with explicit (per particle) data association decisions and feature 
management methods. Results show the proposed filter performing similarly 
to FastSLAM in an 'easy' scenario, and considerably outperforming it in a 
'hard' scenario. 
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5.8 Bibliographical Remarks 

As explained in Chapter [5] it is critical for data association algorithms not 
to use falsely declared targets in their hypothesis decision making process. 
In [T7], This is a well known fact in the SLAM community, which has been 
addressed by various researchers. For example, M. Montemerlo et. al. use 
methods from occupancy-grid based robotic mapping |12] to estimate the 
number of landmarks in the map state. A probabilistic evidence for landmark 
existence is derived from the measurement and propagated through a Log- 
Odds discrete Bayes recursion. This discrete recursion is independent of the 
main RM state estimation filter and is effectively a post-processing of the map 
state estimate at each time step. The output of the maximum likelihood data 
association decision is incorporated into the existence update. If the landmark 
is not associated (and therefore assumed undetected) the existence posterior 
is reduced, thus inherently assuming that the probability of detection of all 
the landmarks is unity. Based on a predefined-defined log-odds threshold, 
landmarks are then either added or removed from the map state. The number 
of landmarks in the map state at any given time then gives an estimate of 
the number of landmarks in the map, as every component of the map state 
is assumed to be a valid landmark. However, since in reality, landmarks have 
non-unity detection probabilities, a missed-detection does not always indicate 
the non-presence of the landmark. 

Another method of landmark management was introduced by D. Makarsov 
in [T3] and used in pQ which outlined the so-called 'Geometric feature track 
quality' measure of landmark existence. This measure is inversely propor- 
tional to the innovation between a predicted landmark and the measurements 
and is therefore only updated when measurements are made (and associated) . 
It does not consider the frequent sensor errors in terms of detection uncer- 
tainty and spurious measurements. Other techniques |18j simply use the num- 
ber of successive associations over a fixed set of measurement frames which 
requires both low clutter rates and correct association hypotheses. Again, 
such methods are effectively post/pre-processing techniques which are inde- 
pendent of the state estimating filter recursion. 

Sequential Monte Carlo (SMC) solutions to Bayesian SLAM also gained 
popularity [35] through the use of Rao-Blackwellised particle filters. Fast- 
SLAM [17] displayed impressive results by sampling over the vehicle trajec- 
tory and applying independent Kalman filters to estimate the location of the 
hypothesised map features. By conditioning the map estimates on the history 
of vehicle poses, a conditional measurement independence is invoked which 
allows the correlations introduced in [5] to be discarded. 

A Gaussian mixture solution to the Bayesian SLAM problem was also 
described in |53j which approximated both the transition and measurement 
densities as Gaussian mixtures and propagated the joint state through a 
Bayes recursion. 
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Extensions to of the PHD filter to SLAM were first given in [51] [55] , upon 
which this chapter is based. 



Chapter 6 

Rao-Blackwellised RFS Bayesian SLAM 



6.1 Introduction 

This chapter proposes an alternative Bayesian framework for feature-based 
SLAM, again in the general case of uncertain feature number and data as- 
sociation. As in Chapter [5j a first order solution, coined the probability hy- 
pothesis density (PHD) SLAM filter, is used, which jointly propagates the 
posterior PHD of the map and the posterior distribution of the vehicle tra- 
jectory. In this chapter however, a Rao-Blackwellised (RB) implementation 
of the PHD-SLAM filter is proposed based on the GM PHD filter for the map 
and a particle filter for the vehicle trajectory, with initial results presented 
in [56] and further refinements in [57j . 

A tractable PHD approximation to the SLAM problem is derived, which 
propagates the posterior PHDs of multiple trajectory-conditioned maps and 
the posterior distribution of the trajectory of the vehicle. Furthermore, this 
approach to SLAM admits the concept of an 'expected' map via the PHD 
construct, which is not available in previous SLAM approaches. 

The chapter is organised as follows. Section 16.21 discusses the factorised 
RFS SLAM recursion, in which the posterior density of the map, conditioned 
on the trajectory, and the trajectory itself can be propagated jointly. The 
RFS framework is then applied to this factorised solution, where it is demon- 
strated that subtle differences, regarding the use of sets, make a direct, naive 
implementation of FastSLAM to the RFS problem inappropriate. In particu- 
lar, the likelihood of the measurement, conditioned on the trajectory, which 
is necessary for the calculation of the particle weights, cannot be approxi- 
mated under an EKF framework, as in FastSLAM [58.. Solutions, which give 
a closed form solution to this problem, are presented in this section. Section 
16.31 outlines a Rao-Blackwellised implementation of the PHD-SLAM filter. 
The necessary steps to implement the PHD filter for the estimation of the 
map and the vehicle trajectory are given, along with pseudo code. Section 
16.41 presents and discusses the Rao-Blackwellised RFS SLAM performance. 
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Demonstrations of simulated examples are given, due to the simplicity of 
generating both trajectory and map ground truth values, necessary for true 
performance evaluation assessment. This is followed by an implementation 
with real, short range, millimetre wave (MMW) radar data, and a mobile 
robot platform, in a car park environment. The advantages of these sensors 
over other devices such as laser range finders is discussed in [IB]. Data sets 
from the radar are recorded, along with odometry and single axis gyro rate 
data, from a moving vehicle. Comparisons are made with classical, vector 
based, EKF SLAM which utilises the Joint Compatibility Branch and Bound 
(JCBB) |3S] data association method and FastSLAM with Multiple Hypoth- 
esis (MH) data association. Further comparative results, in a much larger 
scenario, where accurate SLAM performance in the presence of high clutter 
is essential, are demonstrated at sea, in a coastal environment, using an "Au- 
tonomous kayak" [59] as the vehicle and a commercially available X-Band 
radar. The performance improvement, in the presence of clutter is clearly 
demonstrated. Comparisons and discussions of the computational complex- 
ity of the algorithms is also given. 



6.2 The Rao-Blackwellised (RB) PHD-SLAM Filter 

Since the full RFS-SLAM Bayes filter of equations 12.141 and 12.151 is numer- 
ically intractable, it is again necessary to look for tractable but principled 
approximations. This section derives a recursion that jointly propagates the 
posterior PHD of the map and the posterior density of the vehicle trajec- 
tory. Analogous to FastSLAM, the RFS-SLAM recursion can be factorised 
as shown in Section f6. 2. II Section [6.2.21 discusses the PHD estimator in the 
context of this factorised recursion, Section lB.2.3l addresses the PHD represen- 
tation of the map component only while Scction [6.2. 41 extends this algorithm 
to perform SLAM. 



6.2.1 The Factorised RFS-SLAM Recursion 

Using standard conditional probability, the joint posterior RFS-SLAM den- 
sity of equation 12. 151 can be decomposed as, 

Pk(Mk,X 1:k \Z :k,Uo:k-l,X ) = 

p k (X 1 .. k \Z .. k ,Uo:k-l,X ) Pk (M k \Z :k,X .. k ). (6.1) 

Thus, the recursion for the joint RFS map-trajectory posterior density ac- 
cording to equation 12.151 is equivalent to jointly propagating the posterior 
density of the map conditioned on the trajectory and the posterior density 
of the trajectory. In this section, as before, for compactness, 
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Pk\k-l(M k \X :k) = Pk\k-l(M k \Z Q :k-l, X Q .. k ) (6.2) 

Pk(M k \X Q:k ) = Pk (M k \Z Q:k ,X Q .. k ) (6.3) 

Pk(X 1: k)=Pk(Xuk\Zo;k,U ..k-i,X ) (6.4) 

and it follows that, 

Pk\k-i(M k \X :k) = / fM(Mk\Mk-i,X k )pk-i(Mk-i\Xo:k-i)SMk-i 

(6.5) 

, u , v . .9fe(2fc|M fe ,X fc K.| fe _ 1 (7W fc |Xo: fc ) 
p fc (7W fe |X 0:fc ) = — — — ! — — (6.6) 

g k {£ k \£0:k-l, JL0:k) 

, v >. ,„ | 7 v ^fx(Xk\X k -i,Uk-i)Pk-i{Xuk-i.) (a ^ 
p k {X 1:k ) = g k (Z k \Z .. k -i,X .. k ) . (6.7) 

gfci-s/clAhfc-i] 

Apart from adopting RFS likelihoods for the measurement and map, the 
recursion defined by equations 16.51 16.61 and 16.71 is similar to that in Fast- 
SLAM [35], J5U] ■ However, the use of RFS likelihoods has important conse- 
quences in the evaluation of equation l6.71 to be seen later in Section [6. 2 .41 In 
FastSLAM, it should be noted that the map recursion of equation 16. 61 is fur- 
ther decomposed into the product of K independent densities for each of the 
K features assumed to exist in the map. Furthermore, FastSLAM is condi- 
tioned on the inherently unknown data association assignments. In contrast, 
RFS-SLAM is not conditioned on any data association hypotheses to deter- 
mine the number of features to estimate and the recursion of equation 16.61 
is that of a RFS feature map. Consequently, the propagation of the map in- 
volves probability densities of random finite sets and marginalisation over the 
map involves set integrals. Similar to FastSLAM, the effect of the trajectory 
conditioning on RFS-SLAM is to render each feature estimate conditionally 
independent and thus the map correlations, critical to EKF-SLAM [I], are 
not required. 



6.2.2 The PHD in RFS-SLAM 

Recall fronr Section 13.3.11 that an optimal estimator for a random vector is 
the conditional expectation. Many state-of-the-art SLAM algorithms adopt 
Sequential Monte Carlo (SMC) methods. It is well known that SMC tech- 
niques are more amenable to expectation operations than maximisation oper- 
ations, since if p is approximated by a set of weighted samples {rf % \ X( l '}^ =1 , 
then [HI], [52], 

N 

J2 rj^X W -> ELY] (6.8) 
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as iV— >oo. However, in FastSLAM [55], it is common to choose the trajectory 
sample with the highest weight as the estimate of the vehicle path, and its 
corresponding map, as the estimate of the map. It is easier to establish con- 
vergence in SMC implementations if we use the expected path and expected 
map. However, it is not clear how the expected map is interpreted. 

The PHD construct allows an alternative notion of expectation for maps, 
thereby fully exploiting the advantage of an SMC approximation. The PHD, 
v, is a function defined on the feature space satisfying equation 13.181 Recall 
from Scction l3.3.4.1l that the value of the PHD at a point gives the expected 
number of features at that point while the integral over any given region 
gives the expected number of features in that region. A salient property of 
the PHD construct in map estimation is that the posterior PHD of the map is 
indeed the expectation of the trajectory-conditioned PHDs. More concisely, 

v k {m) = E[v k (m\X 1:k )], (6.9) 

where the expectation is taken over the vehicle trajectory X\- k . This result 
follows from the standard properties of the PHD (intensity function) of an 
RFS, see for example classical texts such as [31], [35]. Thus the PHD con- 
struct provides a natural framework to average feature map estimates, while 
incorporating both unknown associations and different feature numbers. This 
differs dramatically from vector based map averaging methods which require 
feature identification tracking and rule-based combinations [63] . In contrast, 
map averaging for grid-based maps is straight forward due to both known 
grid alignments and number of cells. While the practical merits of an ex- 
pected feature map estimate for SLAM using a single sensor may be unclear 
at this time, related operations such as 'feature map addition' may be of 
use in sensor fusion and multi-robot SLAM applications. Furthermore, the 
PHD construct admits a Bayes optimal estimator for the map, as discussed 
previously in Section 13.3.11 



6.2.3 PHD Mapping 

This section details the trajectory-conditioned PHD mapping recursion of 
equation 16.61 as was first proposed in [64] . The predicted and posterior RFS 
maps are approximated by Poisson RFSs with PHDs v k \ k _x{m\Xo: k ) and 
v k (m\X Q:k ) respectively, 

I\ v k\ k -i(m\ x 0:k) 

Pk\k-i(M k \X 0:k ) » f- (6.10) 

exp(J v k \ k -i(m\X 0:k )dm) 
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1] v k (m\X Q . k ) 
Pk(M k \X a .. k ) » m ^ k (6.11) 

In essence, this approximation assumes that features arc IID and the number 
of features is Poisson distributed. This PHD approximation has been proven 
to be powerful and effective in multi-target tracking [3J. Poisson approxi- 
mations for the number of new features have also been adopted in certain 
SLAM solutions [T~4| . In light of the above advantages of representing an RFS 
with sequential Monte Carlo methods, the PHD filter for the SLAM problem 
can be implemented in Rao-Blackwellised form. Again, referring to the PHD 
predictor - corrector of equations 13.231 and 13.241 substituting 

A — ► m\X 0:k (6.12) 

the PHD predictor equation then becomes 

v k \ k -i(m\ x 0:k) = Vk-i(m\X :k-i) +b(m\X k ) (6.13) 

where b(m\X k ) is the PHD of the new feature RFS, B(X k ), initially intro- 
duced in Section 13711 

The corresponding Rao-Blackwellised, PHD corrector equation is then 



v k (m\X ., k ) = Ufc|fc_i(m|X 0: fe) 1 - P D (m\X k ) + 

\p P D (m\X k )g k (z\m, X k ) 

J^ c k (z\X k ) + fP D (Z\X k )g k (z\Z,X k )v k \ k - 1 (S\X , k )dS 

where 



(6.14) 



Pjj(m\Xk) = the probability of detecting a feature at 

to, from vehicle pose X k . 
c k {z\X k ) = PHD of the clutter RFS C k (X k ) (in equation [2~TUjl (6.15) 

at time k and, 
g k (z\m,X k ) = the measurement model of the sensor at time k. 

In contrast to the "Brute force" SLAM approach of chapter the RB PHD 
SLAM filter estimates multiple, conditionally independent PHDs (intensity 
functions). Each independent map PHD, is conditioned on each of the N hy- 
pothesised vehicle trajectories (particles). Referring again to the GM example 
representations of PHDs in figures 13721 and I3~3l in any particular map PHD, 
each Gaussian representing a/some possible feature (s) is conditioned on one 
of the N hypothesised vehicle trajectories. A^ such conditionally independent 
PHDs, one per hypothesised trajectory, are then propagated. 
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6.2.4 PHD-SLAM 

This section extends the trajectory-conditioned PHD mapping recursion to 
the SLAM problem. With the hindsight of FastSLAM jaS], the most obvious 
extension of PHD mapping [M] to SLAM is to exploit the factorisation equa- 
tions [131152] and [HIZl e.g. PHD for mapping and particle filtering for localisa- 
tion. This technique requires the computation of the posterior density of the 
vehicle trajectory in eauation l6.7l in particular the term g k (Z k \Zo :k -i, ^o-.k), 
which requires set integration, 

g k (Z k \Zo:k-l,Xo:k)=[p(Zk,M k \Zo:k-l,X : k )6M k . (6.16) 

This set integral is numerically intractable and a naive approach is to directly 
apply the EKF approximation proposed for FastSLAM 65] . However, an EKF 
approximation cannot be used since the likelihood co.uation l6.16l defined on 
the space of finite-sets, and its FastSLAM counterpart, defined on a Euclidean 
space, are two fundamentally different quantities and it is not known how they 
are even related. Therefore, in this case, it is fundamentally incorrect to use 
the EKF approximation in [58j as it will not result in a valid density, and 
thus its product with equation 16 . 61 cannot give the joint posterior of the map 
and pose. An EKF approximation requires a hypothesised data association 
assignment. Since there is no concept of data association in the RFS-SLAM 
framework (there is no fixed ordering of features or measurements in the 
state), alternative methods of evaluation of equation 16. 161 are required. 

Fortunately, by rearranging equation 16.61 it can be seen that 
g k (Z k \Za; k -i, Xo : k) is merely the normalising constant, 

,- |_ v v gk(Zk\M k) XkjpQk-xiMklXo-.k) /C1 „, 

gk{Zk\Zo:k-i,X .. k ) = . (6.17) 

Pk{Mk\X .. k ) 

Note in the above, that the LHS does not contain the variable A4 k , while 
the RHS has M. k in both the denominator and numerator. In essence, M. k 
in equation 16.171 is a dummy variable, and thus equation 16.171 holds for any 
arbitrary choice of M. k - This allows the substitution of any choice of M. k to 
evaluate g k (Zk\Zo : k~-i,Xo :k )- This is an important result, which allows for 
the likelihood of the measurement conditioned on the trajectory (but not the 
map), to be calculated in closed- form, as opposed to using the EKF approxi- 
mations in [55]. The following considers two simple choices: (derivations can 
be seen in Appendix IBl) 



6.2.4.1 The Empty Strategy 

It was mentioned in Section 13.3.4.41 that if the RFS M. k is Poisson dis- 
tributed in its number, and the points within M. k arc IID distributed, then 
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the probability density of M. k can be recovered exactly from the PHD inten- 
sity function according to eouation l3.19l Similarly the predicted and posterior 
RFS maps can be approximated by Poisson RFSs with PHDs v k \ k ^i(m\X 0:k ) 
and Vk{m\Xo-k) respectively, 

EI U fc|fc-l(H^O:fc) 

Pk\k-i(M k \X .. k ) » -=^i (6.18) 

1] Ufc(m|X 0:fc ) 

Pk (M k \X .. k ) » "7^ fc . - - v (6.19) 

exp(J w fc (m|A :fc)dmJ 

Setting A^fc = 0, and using the Poisson RFS approximations, equation 16. 181 
and eauation l6.191 as well as the RFS measurement likelihood, equation 15.41 
shown in Section 15.21 it follows that (see Appendix [Bj 



gk(Zk\2o;k-i,X ;k) ~ ftf* exp f m k -m fc | fc _i - / c k (z\X k )dz 



(6.20) 



where, n k k = ] [ c k (z\X k ) with, Cfc(z|Xfc) being the PHD of the measure- 

z£Z k 

ment clutter RFS C k (X k ). In addition, m k = J v k (m\Xo: k )dm and ttlfeifc— l = 
/ v k\k-i{ rn \^0:k)dm. Equation 16.201 gives the closed form likelihood of the 
measurement, conditioned on the trajectory, and not on the map. 



6.2.4.2 The Single Feature Strategy 

In a similar manner, to evaluate the normalising constant for the case of 
■Mk — {fh}, again using equations 16. 181 l6~l"9l and 15.41 

gk(Zk\Zo:k-l,Xo:k) ~ y; 



{\-P D {fh\X k ))4 k + 



P D (m\X k ) ^2 K k" {z} 9h(z\m ) X k ))vk\k-i('m\X 0: k) 



(6.21) 



with, 

r = exp f m fe | fe _i -rfifc + c k (z)dzj v k (m\X 0:k ). (6.22) 

For this choice of A4 k , m can be, for instance, the feature with the least uncer- 
tainty or the maximum measurement likelihood. It is possible to choose M. k 
with multiple features, however this will increase the computational burden. 
Due to the presence of the measurement likelihood term g k {z\fh,X k ), it is 
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expected that, in general, the single feature map update will outperform that 
of the empty map update. Note that in equation 16.171 every choice of Aik 
would give the same result, however equations 16. 201 and 16. 211 use different ap- 
proximations oi pk(Mk\Xo-,k), yielding slightly different results. In principle, 
any map strategy can be used including more features, however the computa- 
tion required to evaluate the trajectory conditioned measurement likelihood 
would also increase. The following section presents a Rao-Blackwellised im- 
plementation of the proposed PHD-SLAM filter. 



6.3 Rao-Blackwellised Implementation of the 
PHD-SLAM Filter 

Following the description of the PHD-SLAM filter in the previous section, a 
Rao-Blackwellised (RB) implementation is detailed in this section. In essence, 
a particle filter is used to propagate the vehicle trajectory (equation l6.7|) . and 
a Gaussian mixture (GM) PHD filter is used to propagate each trajectory- 
conditioned map PHD (equation 16. 6p . As such, let the PHD-SLAM density 
at time k— 1 be represented by a set of N particles, 

N 



W vW w ( m \x w ) 

where -X"o*fe-i = 1^°' -^l 1-^2 > ■ ■ ■ > -^fc-i] * s * ne i ,th hypothesised vehicle tra- 
jectory and Wj.„ 1 (f7T,|ATp^_ 1 ) is its map PHD. The filter then proceeds to 
approximate the posterior density by a new set of weighted particles, 



•^4t«£°Hx«) 



N 



as follows: 



6.3.1 PHD Mapping - Implementation 

Let the prior map P. 
mixture of the form, 



Let the prior map PHD for the i th particle, %y£_x{'m\Xk_- i ), be a Gaussian 



r(<) 

'k-i 



&n*i) = E »£$"(** vtl,*£$) (6-23) 
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which is a mixture of J^_ x Gaussians, with oj^f{, /U^-i an< ^ -^fe-i being the 
corresponding predicted weights, means and covariances respectively for the 
j th Gaussian component of the map PHD of the i th trajectory. Let the new 
feature intensity for the particle, b(m\Zk~i,X k ), from the sampled pose, 
X¥ at time k also be a Gaussian mixture of the form 

41 

b(m\Z k ^X^) = E^W(m;^,^) (6.24) 

i=i 

where, Jj^ k defines the number of Gaussians in the new feature intensity at 

time k and w^ [ , /i£ 'I and Pu % u are the corresponding components. This is 
analogous to the proposal distribution in the particle filter and provides an 
initial estimate of the new features entering the map. 

The predicted intensity is therefore also a Gaussian mixture, 



J k\k-1 



"SUM^n- 22 ^tM^^ZvP^) (6-25) 

which consists of J^ k _ 1 = Jk-i + ^bl Gaussians representing the union of 

the prior map intensity, Ufc_i(m|X / [,_ 1 ), and the proposed new feature inten- 
sity according to equation 16.131 Since the measurement likelihood is also of 
Gaussian form, it follows from equation 16.141 that the posterior map PHD, 
v k {m\X k l ) is then also a Gaussian mixture given by 



vt\m\X^) = v%_ x (m|xW) 



z£Z k 3=1 



(6.26) 



The components of the above equation are given by, 



:%>(z,m\XZ>) = ^m V W(rn-,^f k >,P^>) (6.27) 

^i 3) (z\X { k l) ) = (<) k]k - V k]k - ; (6.28) 

C (z) ^M^H.v^^se,,^) 



£=1 



The terms /ifcifc, Pjuj. and Sfc can be obtained using any standard filtering 
technique such as EKF or UKF [55]. In this chapter, the EKF updates are 
adopted. 
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The clutter RFS, C k , is assumed Poisson distributed [H] in number and 
uniformly spaced over the mapping region. Therefore the clutter intensity is 
given by, c(z) = X c U(z), where A c is the average number of clutter measure- 
ments and 1t(z) denotes a uniform distribution on the measurement space. 
As with other feature-based GM implementations [35J , pruning and merging 
operations are required to curb the explosive growth in the number of Gaus- 
sian components of the posterior map PHD. These operations are carried out 
as in 1551. 



6.3.2 The Vehicle Trajectory - Implementation 

The proposed filter adopts a particle approximation of the posterior vehicle 
trajectory, Pk{Xi-k), which is sampled/re-sampled as follows: 

Step 1: Sampling Step 

• Fori = 1,...,N, sample Xj? ~ fxiXPlX^Uk-i) and set 



g fc (Z fc |Z 0:fc _ 1 ,A:^)/^(4 t) |X< t 2 1 , U k -: 

Mx^x^u^) 



~(i) _ yk\*k\*0:k-l,* 0:k )JXK*k l^fc-l.^fc-U m (i) , . 



• Normalise weights: X)»=i % = 1- 
Step 2: Resampling Step 



N { ... ,.s-,iV 



Resample {^ } , X« } , =i to get {r?« , X« }. =1 



Since the vehicle transition density is chosen as the proposal density as with 
FastSLAM 1.0 [58], 

rf^ =g k {Z k \Z .. k ^X^)4l x (6.30) 

which can be evaluated in closed form according to Mk being the empty map 
(equation 16. 20[) or M. k being a single feature map (equation 16. 21[) . where 



3=1 J=l 

Note that the incorporation of the measurement conditioned proposal of Fast- 
SLAM 2.0 can also be accommodated in this framework. This direction of 
research focuses on more efficient SMC approximations and is an avenue for 
further studies. 
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6.3.3 SLAM State Estimation and Pseudo-code 

As mentioned in the introduction, in contrast to previous SLAM algorithms, 
the PHD map representation allows for a natural ability to average feature 
maps. That is, independent map estimates from N independent trajectory 
particles can be readily averaged into an expected estimate, even with map 
estimates of different size and without having to resolve the intra-map feature 
associations. Consequently, in the case of the RB-PHD-SLAM filter, both the 
expected vehicle trajectory and feature map can be determined as follows: 
Given the posterior set of weighted particles and corresponding map PHDs, 

r 1 N 

and fj = X)»=i Vk tnen > 

*o :fe = ~f>M- (6-32) 



i=i 



As demonstrated previously in Section 16.2.41 the posterior PHD of the map 
is the expectation of the trajectory-conditioned PHDs and thus 

1 N 
v k (m\X 0:k ) = -X^M'VPr&l)- (6-33) 

^ 4=1 

If ififc = J Vk(m\Xo : k)dm, is the mass of the posterior map PHD, the expected 
map estimate can then be extracted by choosing the tfifc highest local maxima. 
The pseudo-code for the RB-PHD-SLAM filter are provided in tables l6~Tll6~2l 
while that of appropriate estimators is provided in Tables 16.31 and 16.41 which 
continues as Table ROl The following section presents the results and analysis 
of the proposed filter, with comparisons to standard SLAM algorithms. 



6.4 Results and Analysis 

This section presents the results and analysis of the proposed approach us- 
ing both simulated and real experimental datasets. Initial comparisons are 
made with the FastSLAM [58] algorithm with maximum likelihood data as- 
sociation, using a mutual exclusion constraint and a 95% x 2 confidence gate. 
These comparisons are demonstrated with a simulated single loop vehicle 
trajectory carrying a simulated range - bearing measuring sensor and a real, 
land based vehicle using a millimetre wave (MMW) radar for feature extrac- 
tion. To further demonstrate the abilities of the RB-RFS-SLAM approach, 
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Table 6.1 RB-PHD-SLAM-Predict 



Algorithm RB-PHD-SLAM-Predict 




({&Ai.Aw^a!°.i)}£ 1 .2»-i I M 




// Construct (|6.25|) 




1. for i = 1 to N do 




// Sample pose 




2. X® ~ f x {X^\X<H v U k _ x ) 




II Predict map 




3. GMM-PHD-FBRM-Predict {Z k -x,X^_ ly v^{m\Z k . 


y(«) \ 


4. end for 




5. return ({!,&, X» V% _ x {m\Z k ^ X^_ x )}f =l ) 





Table 6.2 RB-PHD-SLAM-Update 



Algorithm RB-PHD-SLAM-Update 




{{vti^t\4l-x^\Z k ^X^ 1 )}l 1 ,Z k ) 




1. for i = 1 to AT do 




// Update map PHD 




2. GMM-PHD-FBRM-Update (Z k ,X%\v$ h _ 1 (m\Z k . 


-i.*£i)) 


// Predicted PHD mass 




7 (i) r -i 

3- mfc|fc-i - 2^=i w fe| fc -i 




// Updated PHD mass 




4. m fc - 2^=i w fc 




5. if( Empty Strategy TRUE ) do 




// Updated traiectorv weieht of (|6.20|) 




6. 4 4) = (c(^) |Zfe| exp(* fc -**i*-i-*e) )t ? ^ 1 




7. end if 




8. if( Single Feature Strategy TRUE ) do 




// Select a given map state 




9. j = {z = l,...,J j f ) |m (i ' j) =m} 




10. a = (l-P D )c(z)l^l+P D a;^' ) 1 x 




Ew^wm.^) 




ze2 fc 




11. 6 = cxp( A '=ifc-i-^+^) W W) 




// Update traiectorv weight of (|6.2ip 




12 - % = ^^iifc-i 




13. end if 




14. end for 




15. return ({ff ,X«,z;«(m|Z fe , X fc )}£i) 
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Table 6.3 RB-PHD-SLAM-MAPestimate 



Algorithm RB-PHD-SLAM-MAPestimate 

({^l,^fc l) ,4|Ll( m l Z fe-l' X fe-l)}^l' Z fe' T fcaturc) 

// Initialise the map estimate 

1. M k = 

2. I = {1,...,N} 

// Get index of max weight component 

3. j = argmaxr/^, 

iei 
II Estimated Trajectory 

4. X -.k = X Q:k 

II Estimate Map from corresponding map PHD 

5. for i = 1 to 4 J) do 



6. if Ji A > T t 



/,■ 



(feat 



nee 



// concatenate estimate 

7. M k = [M k $'*>] 

8. end if 

9. end for 

// RB-PHD-SLAM MAP Estimate 

10. return (X Q:k ,M k ) 



further, somewhat more complicated, experiments are carried out in which 
the benchmark algorithms used are the classical FastSLAM [55] but with 
with Multiple Hypothesis Data association [57] and the Joint Compatibility 
Branch and Bound (JCBB) EKF [39]. In this second set of experiments, in 
the simulation, multiple vehicle loop trajectories are executed and for the real 
experiment, a much larger scenario, where accurate SLAM performance in 
the presence of high clutter is essential, is demonstrated at sea, in a coastal 
environment, using an "Autonomous kayak" [SH] as the vehicle and a com- 
mercially available X-Band radar. 

In all experiments, the 'single feature map' trajectory weighting of equation 
16.211 is adopted for the proposed RB-PHD-SLAM filter. An implementation 
using the 'empty map update' of equation 16.201 was presented in [55]. While 
any feature can theoretically be selected to generate the trajectory weight- 
ing, in this implementation, that which generates the maximum likelihood 
amongst all measurements is chosen. A comprehensive study as to the best 
suited feature selection strategies is left to future work. 

Current SLAM filters deal with clutter through 'feature management' 
routines, such as the landmark's quality [TJ or a binary Bayes filter [55] . 
These operations are typically independent of the joint SLAM filter update, 
whereas the proposed approach unifies feature management, data association 
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Table 6.4 RB-PHD-SLAM-EAPestimate 



Algorithm RB-PHD-SLAM-EAPestimate 

W-li4l ,, i|LlH^-lJlt-l)}fcl,2k 1 Tfcaturc, Anin) 

1. Q k = 

2. for i = 1 to N do 

3. Q k = Q k + r$ 

4. end for 

// expected trajectory 

i N 

z— 1 

// Initialise number of Gaussian components 

6. 1 = 

7. for i = 1 to N 



8. 


for j = 1 to 4 4) 


9. 


1 = 1+1 


10. 




11. 


-(0 (*.i) 
A4 = /4 


12. 


p(0 _ p(M') 


13. 


end for 



14. end for 

15. ft = {1, . . . , J} 

// Initialise number of merged Gaussian components 

16. L = 

// Gaussian merging 

17. do while 11 ^ 

// Increment component counter 

18. L = L+l 

// Get index of max weight component 

- (r) 

19. j = argmaxwj. 

// Cluster those within distance D m i n 

20. K = {r e U\(-^ - H^riPfY 1 ^ ~ fi?) < Anln} 
// Combine component weights 

21. 4 L) = £<^ 

II Weighted average mean 

29 f, (L) - 1 YVi^ fiW 

^- ^fc - T7I)" Z^ w fc Mfc 
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Table 6.4 (Continued) 



I J Combined covariancc 

// Remove K from 1Z and repeat 

23. Pi L) = ^ E & (3° + (A L) - A*)<»P - tf> 

24. 11 = 11- K 

25. end while 

26. for i = 1 to L do 

27. if tD fc > Tf ea ture 

// concatenate estimate 

28. M k = [M k Ai 8) ] 

29. end if 

30. end for 

// RB-PHD-SLAM EAP Estimate 

31. return (X 0:k ,Mk) 



and state filtering into a single Bayesian update. While these methods have 
been used successfully, they generally discard the sensor's detection (Pd) and 
false alarm (Pfa) probabilities and thus can be erroneous when subject to 
large clutter rates and/or measurement noise. Since the proposed approach 
assumes knowledge of these probabilities, as seen in equation 16.141 a modi- 
fied feature management routine coined the 'feature existence filter' (see Ap- 
pendix [C|. which incorporates both Pn and Pfa, is used with the benchmark 
algorithms in an attempt to be 'fairer' to them in the comparisons. 

To quantify the map estimation error, a metric must be adopted which 
jointly evaluates the error in the feature location and number estimates. 
Current methods typically examine the location estimates of a selected num- 
ber of features and obtain their Mean Squared Error (MSE) using ground 
truth [lj. As such, vector-based error metrics are applied to feature maps 
and the error in the estimated number of features is neglected. While there 
are several metrics for finitc-set-valued estimation error, that of [23] has been 
demonstrated to be most suitable [51], [56 . Therefore, the set map error 
metric described in Chapter 3] (equation I4.6f) is therefore once again used 
to gauge the mapping performance in terms of estimated and actual feature 
number, as well spatial error. In the following sections, this metric along with 
the root mean squared error (RMSE) and graphical comparisons are used to 
demonstrate the merits of the RB-PHD-SLAM filter. 
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6-4-1 Simulated Datasets 

Comparisons of RB-RFS-SLAM with standard vector based SLAM algo- 
rithms are firstly presented in the form of simulated trials due to the ease of 
generating known ground truth (trajectories and maps) for estimation error 
evaluation. 



6.4.1.1 Simulation: Single Loop Trajectory 

The filter parameters used for the single loop trajectory simulated trial are 
shown in Table 16.51 The measurement noise was inflated to hinder data as- 
Table 6.5 Filter parameters used for the single loop trajectory trial. 



Filter Parameter 


Value 


Velocity input standard deviation (std) 


1.5 m/s 


Steering input std. 


9.5° 


Range measurement std. 


1.75m 


Bearing measurement std. 


3.5° 


Probability of Detection Po 


0.95 


Clutter rate A c 


5m" 2 


Sensor maximum range 


10m 


Sensor Field-of-View (FoV) 


360° 


Feature existence threshold 


0.5 



sociation in the vector-based filter. For both filters, both the maximum a 
posterior (MAP) and expected a posterior (EAP) trajectories are reported. 
For FastSLAM, the map of the highest weighted trajectory estimate is used 
as the map estimate, while for the proposed filter, both the map of the highest 
weighted trajectory and the EAP map estimate are determined for compari- 
son. 50 Monte Carlo (MC) trials were carried out. 

Figure 16.11 shows a sample of the raw data used in the trials, with the 
green circles depicting the true feature locations. A quantitative evaluation 
of the estimation results is provided through the RMSE, along with stan- 
dard deviations, of the trajectory estimate as shown in figure 16.21 Without 
knowledge of Pjj and Pfa, the benchmark approach can be made to ap- 
pear highly erroneous due to poor feature management. Incorporating this 
information can improve the result, however the feature management is still 
effectively a post-filter update processing method. The RB-PHD-SLAM algo- 
rithm is significantly more robust due to the RFS feature map representation 
and Bayesian recursion which jointly performs feature management and state 
estimation. 



6.4 Results and Analysis 
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Fig. 6.1 The simulated environment showing point features (green circles). A sam- 
ple measurement history (black points) plotted from a sample noisy trajectory (blue 
line) is also shown. 



In terms of the map estimation, figure RT51 depicts both the true and esti- 
mated number of features as the robot explores the map, with the proposed 
approach closely tracking the true number. Note that since this trial is a 
simulation, the true number of features which have entered the vehicle's FoV 
during its entire trajectory, can be calculated exactly. Since this result does 
not examine the locations of the estimated features, the set metric of equa- 
tion 2jJ] is used to compare map estimates, as shown in figure ROI The figure 
shows the 'ideal' mapping error (i.e. every feature is instantly estimated by 
its true coordinates when it enters the sensor FoV), which converges to zero 
once all features in the map have been scanned. The mean and std of the 
map estimates for both the benchmark and proposed approach are plotted, 
with that of the RB-PHD-SLAM filter reporting less map estimation error. 
A qualitative depiction of the posterior estimates from both approaches is 
provided in figures RT5l and UTol demonstrating the usefulness of the RFS ap- 
proach and the associated RB-PHD-SLAM filter. In both figures, the green 
line and circles represent the ground truth vehicle trajectory and feature lo- 
cations respectively. The black crosses represent the estimated map. In the 
case of FastSLAM, this is derived with respect to the MAP FastSLAM trajec- 
tory estimate (the particle (trajectory) with the final maximum weight). In 
each figure, the blue line indicates the MAP trajectory estimate, which cor- 
responds to the particle with the maximum weight, at each time and the red 
line corresponds to the expected trajectory estimate, which is the weighted 
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average of all particles at each time (sec Tables RT51 and l(0| . Given that the 
filter jointly incorporates data association and feature number uncertainty 
into its Bayesian recursion, it is more robust to large sensing error, as it does 
not rely on hard measurement-feature assignment decisions. Furthermore, it 
jointly estimates the number of features and their locations, alleviating the 
need for popular feature management methods pQ, [55] . 
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Fig. 6.2 The mean and standard deviation of the expected trajectory estimates 
of RB-PHD-SLAM vs. that of FastSLAM over 50 MC runs. LQ refers to an imple- 
mentation with the 'landmark quality' method of [T|. 
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Fig. 6.3 The average estimated number of features in the map vs. ground truth 
for each approach. The feature number estimate of RB-PHD-SLAM can be seen 
to closely track that of the ground truth. Clearly there is no distinction between 
correctly estimated feature and false feature in this result. 
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Fig. 6.4 A comparative plot of the mean and standard deviation of the map es- 
timation error vs. time. The error incorporates that of the number of features, 
shown in figure 16.31 as well as their positional estimates. Note that the 'ideal' er- 
ror converges to zero, an important property for SLAM filters and map estimation 
comparisons. 
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Fig. 6.5 A sample posterior estimate from FastSLAM showing error in the esti- 
mated trajectory and feature map. The green circles and line show the ground truth 
feature locations and path respectively. The black crosses show the FastSLAM es- 
timated map (feature locations). The blue line shows the MAP trajectory estimate 
and the red line shows the expected trajectory estimate. 
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Fig. 6.6 The posterior estimate given the same inputs / measurements as those 
used in figure 16.51 Again, the green circles and line show the ground truth fea- 
ture locations and path respectively. The black crosses show the RB-PHD-SLAM 
estimated map (feature locations). The RB-PHD-SLAM filter demonstrates its ro- 
bustness and accuracy given high clutter and data association ambiguity. 



6.4.1.2 Simulation: Multiple Loop Trajectories 

The parameters for the more complex, multiple loop trajectory, simulated tri- 
als are shown in tablc l6.6l A 95% validation gate is used throughout. For each 



Table 6.6 Filter parameters used for the single loop trajectory trial. 



Filter Parameter 


Value 


Velocity input standard deviation (std) 


2.0 m/s 


Steering input std. 


5.0° 


Range measurement std. 


1.00m 


Bearing measurement std. 


2.0° 


Probability of Detection Pjj 


0.95 


Clutter rate A c 


20m~ 2 


Sensor maximum range 


10m 


Sensor Field-of-View (FoV) 


360° 


Feature existence threshold 


0.5 



6.4 Results and Analysis 
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SLAM filter, 50 Monte Carlo (MC) trials were carried out in which all meth- 
ods received identical sequences of control inputs and measurements. The 
RB based filters used 50 trajectory particles each, while for MH-FastSLAM 
a maximum limit of 2000 particles (number of hypotheses considered prior 
to resampling) was used. 
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Fig. 6.7 The simulated environment showing point features (green circles) and 
true vehicle trajectory (green line). A sample measurement history plotted from a 
sample noisy trajectory (red line) is also shown (black points). 



Figure 16.71 shows a sample of the raw input data used in the trials, su- 
perimposed onto the ground truth feature map and path. A comparison of 
the average trajectory estimation errors for all three filters is then presented 
in Figure 16.81 In terms of the estimated trajectory, the first order RB-PHD- 
SLAM algorithm can be seen to outperform the vector based FastSLAM with 
robust data association, but does not quite achieve the estimation accuracy 
of JCBB-EKFSLAM. This is primarily due to the fact that JCBB is very 
conservative in its choice of measurement-feature associations (jointly com- 
patible constraint) resulting in very few false association pairs influencing 
the trajectory estimation. However, later results in Figures 1631 16. 101 and !6.12l 
highlight the drawbacks of achieving such impressive localisation results. 

In terms of the map estimation component of each SLAM algorithm, Figure 
Hill depicts both the true and estimated number of features as the vehicle ex- 
plores the map, with the proposed RB-PHD-SLAM approach seen to closely 
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track the true number of features in the explored map. Erroneous associa- 
tions for the MH-FastSLAM approach result in excessive feature declarations, 
while the conservative (only including those which are jointly compatible) 
association decisions of JCBB-EKF SLAM reduces the number of correct 
associations. Since vector based feature management routines are typically 
dependant on the data association decisions, this dramatically influences the 
map estimation error. 

Incorporating the estimation error in both the number and location of 
features in the map, Figure 16.101 plots the map error distance of equation 
14.61 for each approach. Note that in order to generate this result, the vector 
based maps of FastSLAM and JCBB-EKFSLAM are temporarily 'assumed' 
to be sets. The proposed method can be seen to report the least mapping 
error due it is robust ability to jointly incorporate uncertainty in feature 
locations and numbers, while erroneous feature estimates contribute to in- 
creased mapping error for the vector based approaches. A qualitative depic- 
tion of the posterior estimates is provided in Figure 16.111 demonstrating the 
usefulness of the RFS-SLAM framework and the proposed RB-PHD-SLAM 
filter. 
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Fig. 6.8 The mean and standard deviation of the trajectory estimates from each 
filter over 50 MC runs versus time. 



6-4-2 A Note on Computational Complexity 



As can be observed from the implementation of Section 16.31 the computa- 
tional complexity of RB-PHD-SLAM is, (D(mkZkN) i.e. linear in the number 
of features (in the FoV) , linear in the number of measurements and linear in 
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Fig. 6.9 The average estimated number of features in the map for each filter versus 
time, compared to the ground truth number of features in the explored map M k . 
The feature number estimate of RB-PHD-SLAM can be seen to closely track that 
of the ground truth. 




Time Index 

Fig. 6.10 A comparative plot of the mean and standard deviation of the map 
estimation error for each filter vs. time. At any given time, for the ideal case, the 
mapping error from equation 14.61 wrt. the explored map is zero. 
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Fig. 6.11 Comparisons of the posterior SLAM estimates from MH-FastSLAM 
(left, red) and the proposed RB-PHD-SLAM (right, blue). The ground truth tra- 
jectory and map are represented by the green line and circles respectively. The 
RB-PHD-SLAM filter demonstrates its robustness and accuracy given high clutter 
and data association ambiguity. 



the number of trajectory particles. The linear complexity of each particle in 
the mapping filter was verified previously in Figure 14.81 

For a single thread implementation, Figure IB . 1 21 shows that the computa- 
tional time is comparable with that of the MH-FastSLAM algorithm, both 
of which are less expensive than JCBB-EKF SLAM as its hypothesis tree 
grows in the presence of high clutter. Note that due to the Rao-Blackwellised 
structure of RB-PHD-SLAM, binary tree based enhancements, such as those 
applied to traditional FastSLAM [55], can be readily developed to further 
reduce the complexity to 0($kNlog(m-k))- Furthermore, in contrast to data 
association based methods, the proposed approach admits numerous other 
computational enhancements, since the map PHD update can be segmented, 
executed in parallel and subsequently fused for state estimation. This is in 
contrast to DA based approaches which are scalable. 



6-4-3 Outdoor Experiments 

6.4.3.1 Land Based SLAM with MMW Radar 



This section discusses the performance of the proposed framework, using a 
millimetre wave radar SLAM dataset in a university car park environment. 
Millimetre wave radar offers numerous advantages over standard laser-based 
systems, returning a power vs. range spectrum. This allows for customised 
detection algorithms to be developed, however it can be prone to high clutter 



6.4 Results and Analysis 



121 




300 400 

Measurement Index 

Fig. 6.12 A comparison of the computation time per measurement update for 
RB-PHD-SLAM (blue), MH-FastSLAM (red) and JCBB-EKFSLAM (black). 



rates [16]. The extracted radar point clusters, plotted relative to the odome- 
try only pose estimates of the vehicle, as well as the odometry pose estimates 
themselves are depicted in figure |6~T31 (left). The information displayed in 
this figure can be thought of as the information input to the SLAM algo- 
rithms, which must be processed to yield the best estimated trajectory and 
map. Given the tree coverage and surrounding buildings in the area, GPS is 
generally not available. Ground truth was thus obtained by manually match- 
ing successive scans from a laser range finder which was also mounted on the 
vehicle, with graphical verification also provided in figure RTTBI (right). The 
vehicle was driven at approximately 1.5m/s around 3 loops, with a control 
input frequency of 10Hz and a radar measurement frequency of 2.5Hz. The 
car park environment is comprised of buildings, bushes, trees, fire hydrants, 
curbs, medians, a car etc. 

Given the small-sized loop, the maximum range of the radar was set at 15m 
and both FastSLAM, with maximum likelihood data association, and RB- 
PHD-SLAM were executed on the dataset. Figure RTP41 depicts the posterior 
estimated trajectory and map using the FastSLAM algorithm (left) and that 
from RB-PHD-SLAM (right), given the same control input samples. Given 
the noisy measurements from the radar sensor, the merits of the proposed 
approach are demonstrated. It should be noted that, as is the case with any 
experimental dataset, the ground truth feature map is extremely difficult to 
obtain, making it challenging to evaluate the feature map estimation error. 
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Fig. 6.13 Left: Odometery and extracted clusters from the radar data, represent- 
ing the raw inputs to the SLAM algorithms. Right: The ground truth trajectory 
(green) obtained by matching laser data due to a lack of GPS data. 
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Fig. 6.14 Left: The posterior estimate from FastSLAM using the radar-based car 
park dataset. Right: The posterior estimate from RB-PHD-SLAM using the same 
dataset. The proposed integrated Bayesian framework for SLAM, incorporating DA 
and feature management enhances the robustness of the SLAM algorithm given 
noisy measurements. 
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6.4.3.2 Sea Based SLAM with X-Band Radar 

This section discusses the filter's performance in a surface-based marine envi- 
ronment, using an X-band radar mounted on a powerboat. In order to max- 
imise the detection of all sea surface point features (comprising boats, buoys, 
etc.), a low detection threshold is required, which subsequently increases the 
clutter rate. GPS data is available for measuring the ground truth trajectory, 
while sea charts and data from surrounding vessels' Automatic Identification 
Systems provide the feature map ground truth. The test site is off the South- 
ern coast of Singapore, as shown in Figure IB. 151 where the boat was driven in 
a loop trajectory of 13Km. Adaptive thresholding methods were applied to 
extract relative point measurements from the radar data |59j . The maximum 
range of the radar, logging at 0.5Hz, was limited to lKm. While heading 
measurements were available via a low grade on-board single axis gyroscope, 
due to the lack of Doppler velocity logs, the speed was estimated at 8 knots 
(4.1 m/s). 




Fig. 6.15 Overview of the test site (1°13' N,103°43' E), showing the GPS trajec- 
tory (green line) and GPS coordinates-ordinates (green dots) of the point feature 
map. The point feature measurement history is also provided (black dots). 



Figure IB" . 161 compares the posterior SLAM estimates from MH-FastSLAM 
and RB-PHD-SLAM, with Figure [6. 171 comparing the estimated map sizes. 
The proposed approach can be seen to generate more accurate localisation 
and feature number estimates, however it can also be seen that some feature 
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estimates are misplaced in comparison to the ground truth feature map. The 
framework is still demonstrated to be useful for high clutter feature-based 
SLAM applications. 
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Fig. 6.16 Top: The posterior SLAM estimate (red) from MH-FastSLAM and Bot- 
tom: The posterior SLAM estimate (blue) from RB-PHD-SLAM, in comparison to 
the ground truth (green). 



6.5 Summary 



This chapter presented a tractable solution for the feature-based SLAM prob- 
lem. The finite set representation of the map admits the notion of an expected 



6.6 Bibliographical Remarks 



125 




500 1000 

Measurement Index 



1500 



Fig. 6.17 Comparison of the number of estimated features for each approach. The 
noisy estimates are likely due to deviations from the Poisson clutter assumption in 
places. 



map in the form of a PHD or intensity function. A Rao-Blackwcllised im- 
plementation of the filter was proposed, in which the PHD of the map was 
propagated using a Gaussian mixture PHD filter, and a particle filter propa- 
gated the vehicle trajectory density. A closed form solution for the trajectory 
weighting was also presented, alleviating the need for approximation, which 
is commonly used. 

Analysis was carried out, both in a simulated environment through Monte 
Carlo trials and an outdoor SLAM experimental dataset based on a millimetre 
wave radar sensor. Results demonstrated the robustness of the proposed filter, 
particularly in the presence of large data association uncertainty and clutter, 
illustrating the merits of adopting an RFS approach to SLAM. 

In terms of its computational complexity, the Rao-Blackwcllised SLAM fil- 
ter was shown to be linear in the number of estimated features, measurements 
and trajectory particles. It should be noted that computational enhancements 
are possible, in terms of parallelisable operations, which are not possible with 
vector based approaches requiring data association. 



6.6 Bibliographical Remarks 



The RFS approach to SLAM was first suggested in [52] with preliminary 
studies using 'brute force' implementations shown in Chaptcr[5J The approach 
modelled the joint vehicle trajectory and map as a single RFS, and recursively 
propagated its first order moment. 
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Initial results of a Rao-Blackwellised (RB) implementation of the PHD- 
SLAM filter, were presented in 56|. This chapter extends [55] . to present a 
more rigorous analysis of the RFS approach to SLAM, an improved version 
of the PHD-SLAM filter as well as real and simulated experimental results, 
and is an extended version of |57| . The merits of the RFS approach are 
demonstrated, particularly in situations of high clutter and data association 
ambiguity. 

A factorised approach to SLAM was established in the, now well known, 
FastSLAM concept [58]. However, this chapter has shown that the same fac- 
torisation method applied to vectors in FastSLAM, cannot be applied to 
sets, since it results in invalid densities in the feature space. Therefore one 
of the main contributions of this chapter is a technique which allows such a 
factorisation to be applied to sets in a principled manner. 



Chapter 7 

Extensions with RFSs in SLAM 



7.1 Introduction 

This book demonstrates that the inherent uncertainty of feature maps and 
feature map measurements can be naturally encapsulated by random finite 
set models, and subsequently in Chapter [5] proposed the multi- feature RFS- 
SLAM framework and recursion of equations 15.51 and 15.61 The SLAM solu- 
tions presented thus far focussed on the joint propagation of the the first-order 
statistical moment or expectation of the RFS map, i.e. its Probability Hy- 
pothesis Density, Vk, and the vehicle trajectory. Recall from Chapter [5] that 
the integral of the PHD, which operates on a feature state space, gives the 
expected number of features in the map, at its maxima represent regions in 
Euclidean map space where features are most likely to exist. 

As will be demonstrated in this chapter, the proposed RFS-SLAM frame- 
work admits numerous alternative approximations and implementations. 
While the PHD-SLAM approach propagates the PHD of the map (encom- 
passing the expected number of features) and the vehicle trajectory, it is also 
possible to append the propagated map PHD with the distribution of the 
number of features, as opposed to just its mean. This can dramatically reduce 
the variance in the estimated number of features in the map when compared 
with the PHD-only approach and subsequently improve the robustness and 
mapping accuracy of PHD-SLAM. 

In contrast to the moment approximation methods of the RFS-SLAM re- 
cursion based on the PHD, a direct approximation of the multi- feature RFS- 
SLAM recursion can also be constructed and propagated. Indeed, by adopting 
a multi-Bernoulli representation of the RFS map, each feature's individual 
probability of existence and probability density, as well as the vehicle tra- 
jectory can be propagated. By modelling each feature's existence probabil- 
ity, this approach is conceptually analogous to existing vector-based SLAM 
approaches which also attempt to incorporate individual feature existence 
estimates. 
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7.2 Alternative RFS Map Representations 

This section presents alternative RFS models for a feature map. The PHD- 
SLAM and RB-PHD-SLAM approaches presented thus far, approximate the 
prior and predicted RFS map densities by Poisson RFSs. Recall from section 
13.3.4.41 that the Poisson RFS can be completely characterised by its PHD, 

I] v k (m\X Q . k ) 

Pk (M k \X Q .. k ) a m ^ k (7.1) 

exp(J v k (m\X .. k )dm) 

Since the mass of the PHD (J" v k (m\Xo :k )dm) is a real number, the cardinality 
of a Poisson RFS map is captured by the single parameter of the denominator 
term, exp(J v k (m\Xo; k )dm) , that is, the distribution of the size of the map is 
also approximated by a Poisson distribution. Since the mean and variance of a 
Poisson distribution are the same, large variations arc likely in the estimated 
number of features for high density maps and/or sensors with large FoVs. 



7.2.1 The Independent and Identically Distributed 
(IID) Cluster RFS 

A more general approximation of the map RFS is an IID cluster process, 

m! Pfe( m ) II v k (m\X Q .. k ) 

Pk (M k \X 0:k ) « - me ^ (7.2) 

[J v k {m\X 0:k )dm) 

where, as before m is the cardinality of the map M. k and now p k (m) is the 
distribution of the size of the feature map. The approximation generalises the 
Poisson RFS representation to allow for any arbitrary cardinality distribu- 
tion, constrained only by the property Y^=o n Pk{ n ) = / v k {ra\Xa: k )dm. It 
can be seen that by replacing p k (m) with a Poisson distribution, (|7.2|) reduces 
to ([7.1JI . Thus the spatial randomness of the features is still encapsulated by a 
Poisson RFS, but the distribution of the number of features is not restricted 
to a Poisson distribution. The subsequent SLAM formulation based on this 
map approximation is presented in section [7.3. II 



7.2.2 The Multi-Bernoulli RFS 

Recall from Chapter |H that an individual feature measurement was mod- 
elled as a Bernoulli RFS, that is, (1) the measurement is not received, with a 
probability equal to that of a missed detection (1 — Pq) (2) the measurement 
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is received with a probability equal to the detection probability of that fea- 
ture and when it is received, has a measurement likelihood P]jg k (z\m, X k ). 
Equivalently, an individual map feature can be represented by a Bernoulli 
RFS which, 

1. does not exist with a probability equal to that of its non-existence proba- 
bility, (1 - P E ), 

2. exists with a probability equal to its existence probability Pe and when it 
does exist, has a spatial likelihood p k {m\Z kl Xq-^). 

Generalising to a feature map comprising m^ features, the multi-Bernoulli 
RFS can be written as 

m k 

Pk (M k =<t)) = l[[l-P i J ) ) (7.3) 



and in the case of any or all n features existing, 

n 

Pk(M k ^{m\...,m n })=p k (<D) J2 



P$ ] p^\mJ\Z k ,X .. k ) 



1 pv 6 j 

l<i 1 ^---jti n <m k j=l 1 r E 



(7.4) 



Note that this map representation is an approximation of the multi-feature 
map posterior density as opposed to a moment-based approximation of other 
approaches. The subsequent SLAM formulation based on this map approx- 
imation is presented in section 17.3.21 Furthermore, while the moment ap- 
proximation methods encapsulate the mean (for the Poisson map model) 
and distribution (for the IDD cluster model) of the number of features in the 
map, the Multi-Bernoulli representation is analogous to popular vector based 
SLAM approaches [55], [T], [T5] in that each feature has its own probability 
of existence. However, by using this RFS representation, the mathematical 
ambiguity highlighted previously in section [2~5l is overcome as the density for 
each feature jointly incorporates the existence and non-existence probabilities 
and still integrates to unity implying it is a valid density function. 



7.3 Extended RB-RFS-SLAM Formulations 

This section details the derivations of the appropriate equations necessary for 
Rao-Blackwellised SLAM implementations based on the extended RFS map 
models of the previous section. 
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7.3.1 RB Cardinalised PHD-SLAM 

In the case of the IID cluster RFS map representation of section 17.2.11 the 
so-called Cardinalised PHD (CPHD) filter [5T] can be readily modified to 
propagate the map PHD and cardinality distribution forward in time as mea- 
surements arrive. If a set of N weighted particles comprising, 



< Particle Weight, Trajectory Sample, Map PHD, Map Cardinality Dist| 



N 



i.e. 

\lU, K.U «2iiM*£i_i). (CMx&Lx)} ( 7 - 5 ) 

is available at time k — 1, then given a vehicle input, Uk-i and measurement, 
Zfe, the RB-CPHD-SLAM filter generates an updated set of particles, 

. N 

4\ JTffl, 4\ra\X^ P f{n\X^)\ (7.6) 

J 4=1 

whereby both the PHD (intensity function) v and the cardinality distribu- 
tion p are updated through predictor - corrector equations via the following 
processes. 



7.3.1.1 Map Update 

~ (i) 

If the predicted vehicle pose, Xj, , is sampled from the vehicle transition 
density fx(Xk\Xk-i, Uk-i) and, as introduced previously in Chapter [4j if 
the prior, wJt_i(wi|-X'fc_i) and birth, V£'(m\x£'), PHDs are available for each 
particle, then for each trajectory sample, the predicted map PHD is, 

•SLiHf) = ^U^xit) + b { k\m\xi% (7.7) 

In addition, if the prior, Pj,_ 1 (n|A^,_ 1 ) and birth cardinality distributions 

Pt, k( n \^-k ) are available, then the predicted map cardinality distribution 
must also be evaluated. Since robotic feature mapping has been formulated 
as a finite-set estimation problem in this book, which was solved by modifying 
the PHD framework in previous chapters, a similar concept can be adopted 
such that the CPHD filtering framework [68] , [51], can be modified to solve 
the feature mapping problem in the case of an IID cluster RFS map. Thus, 
the predicted map cardinality distribution can be obtained from, 
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3=0 V 1=3 J 

(7.8) 

where C\ denotes the binomial coefficient. 

Similarly, given the RFS measurement at time fc, Z kl the updated map 
PHD is then given by, 



E~o n [i jjUMffi , Z k ] (nJpgL^nlXW) 

'i;i-!(-i4 i) ) 



U k \ ln \^k ) ~ oo 0| - (i) ~(j) -, (j) ~(j) W- J ; 



where, 






^, fc (m) = /Cfc( ; l | X % ) N dZ x P D (m\xi?)g k (z\m,X®) (7.10) 

c fe (^l4 ) 



and, 



r^[w fc | fc _ 1 (m|X fc ),Z fe ](n) = 

(J (1 - PD)vfc|fc-i(rn|Afc)dm) 

V4fe - J)-HKMik - J)\^k)Jr J+q 
3=0 



Y] (3fe - JV-pK,k(ik - j)l^fe)-PJV g 

(J Ufc|fe-i(m|A fe )dmJ 



xej^Kin.Zic)). (7.11) 

Here, jfc represents the number of features observed at time k, P? +q denotes 
the permutation coefficient, ej(-) denotes the elementary symmetric function 
of order j and, 



3(vk\k-i,Zk) = { Wk|k-i(m|Xfc)V>a,k(m)dm : z € Z k }. (7.12) 

The updated map cardinality is obtained from, 
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Pk (n\*k ) = ^oo .nr m ; ,^iu „n, ; m — -^TITT ( 7 - 13 ) 



^Wi-i("l^),^]("KVi("l x * ) 
E^,^[«iViH*i <) ),^](n)pif fc _ 1 (n|^ 



7.3.1.2 Trajectory Update 

If, as before, the predicted vehicle pose, X k , is sampled from the vehicle 
transition density, fx(Xk\Xk-i, Uk-x), then the updated weight is calculated 
according to, 



A. The Empty Strategy 



-W _ lk\p k {jk\X^)K k k Pk\k-i(0\X Q:k ) {l) 

h - (jc kiz \x^ dz y k Pk (o\ni) " h - x { ] 

where, as before, n k k = Y[ zeZ Ck(z\Xk) and Ck is the PHD of the RFS 
clutter measurement. Since the map strategy is empty, there are assumed 
no features present for the update and thus the cardinality distribution is 
written as, pk(0\-). 



B. The Single Feature Strategy 



l-P D (m\X®)) K *'-+P D (m\xi e i >) £ 4 h ~*9k{z\fh,X^) 
% = 



exp (jc k (z\X { k ')dz 
pJl^lX^vJl^frnlX^ J vj^rnlX^^ (fl 

pi?(ii4S)«J°(*i43)/t'£|i-iH4S)*n Vk ~ 



where, as before, m, is a feature selected according to a given strategy (i.e. 
least uncertainty, highest measurement likelihood etc.). As before, since the 
strategy is a that of a single feature, the cardinality distribution is written 
as, pfc(l|-)- 

7.3.1.3 Estimator 

As with the RB-PHD-SLAM filter, a MAP estimate of the posterior tra- 
jectory density can be taken to estimate the vehicle pose at each time. To 
estimate the map, EAP or MAP estimates of the number of features in the 
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map can be extracted from the chosen particle's posterior cardinality dis- 
tribution. Conditioned on this estimate, as with the RB-PHD-SLAM filter, 
the feature estimates can be chosen from the peaks of the posterior map 
PHD. Given that the CPHD filter propagates the distribution of the number 
of features as opposed to just its mean, it is anticipated that the map, and 
subsequently the trajectory, estimates from RB-CPHD-SLAM would be re- 
markably improved in comparison to RB-PHD-SLAM. This is an avenue for 
further research. 

7.3.2 RB Multi-target Multi-Bernoulli (MeMBer) 
SLAM 

In the case of the Multi-Bernoulli RFS map representation of section 17.2.21 
the so-called Cardinalised Multi-target Multi-Bernoulli (CMeMBer) filter [5S] 
can be readily modified to propagate the map density forward in time as 
measurements arrive. If a set of weighted particles, 

W 1 N 

•fii. 41i. {(^,£.1. ^iM^Lmfc 1 (7-i6) 

J 4=1 

comprising the particle weight, trajectory sample, the existence probability 
and spatial density of each feature in each trajectory's map, is available at 
time k — 1, then given a vehicle input, Uk-i and measurement, Zk, the RB- 
MeMBer-SLAM filter generates an updated set of particles, 

wl N 
4\ 4- «^ ^M^lmJr (7.17) 

J 4=1 

via the following processes. 



7.3.2.1 Map Update 

If the predicted vehicle pose, Xj? , is sampled from the vehicle transi- 
tion density, fx(Xk\Xk— l, Uk-i), and the parameters of the prior, irk-i = 

{(P^ti^UrnlX^))}^ and birth, {(^, P g(m|A^))}g, 
multi-Bernoulli RFSs are available for each particle, then for each trajectory 
sample, the parameters of the predicted map are obtained from, 

4,Li - *& uun^gH^mli- (7-i8) 
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Again, since the feature mapping problem is cast into a finite-set estimation 
framework, related filters in the tracking community [3j , [69j , can be modified 
to perform the map update. Therefore, given the RFS measurement at time 
k, Zk, the updated map for each sample trajectory particle, X^ , is then 
given by, 

^-{(^.P^H^ffl))}^ (7-19) 

where, J« = J^ + jg + ik . If J«_i = 4-1 + ■$> thcn for 3 = 
1 J W 






E > k ~ ^ fe - 1 1 _ p(«) f p„/ m |y(0ufa-) r m iv0h ■ l j 



" fc ( ' ° :fc) " 1 -lPn(m\X^Um\X^Udm ^ 

p (^) _ ~^ =1 (i-p^-i /p d ('"I^")p1 , !i('"[^I)^) 2 



- E ' fc J {i} pC*' l > fp UlfWwW f„|y(<) 

K fc > 2^=i , D (i.o fD ^,„iv(.),ji] ,„iv(-i 



L 1 /P , r.(m|xW)pi , l 1 (m|xW) fl r h ( a: |m,xW)<Jm 



(7.22) 

E^T 1 (1 ^ . /P g (m|^) gfc (z|m,^)pi'l 1 (m|^)dm 

(7.23) 

7.3.2.2 Trajectory Update 

If the predicted vehicle pose, -Xl , is sampled from the vehicle transition 
density, /x (-Xfel-Xfc-i, Uk-x), then the updated weight is calculated according 

to. 
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A. The Empty Strategy 



z r-r m Mfe-i (-1 _ pd,3) 

fe Bpf/^ftd,) n;ii(i-^:f 



B. The Single Feature Strategy 



1 - P D (m\X®j) Kf + P D (m|X»)E K f- 2 5fc (z|m,l«) 
(i) 7 zeZi, 

% = 



exp 



(; Cfc (z|if)dz) 



l fclfc-l 



X 



fII (i-^)4 £ 



fclfc-i n(i,j) Ai) f-\ v(*) 



P 



^LM) w 



i=l j=l L r E,k-l 



%-l 



where, 



j=i j=i l F E,k 



7.3.2.3 Estimators 

As with the RB-PHD-SLAM filter, an MAP estimate of the posterior trajec- 
tory density can be used to estimate the vehicle pose at each time. Given that 
the posterior existence probability of each feature is available, feature map 
estimate can be obtained from the means or modes of the posterior densities 
of each hypothesised map state with an existence probability greater than a 
given threshold. Using a Bernoulli RFS to represent each feature allows for 
the joint encapsulation of its existence probability and location in a single 
PDF, in contrast to existing SLAM methods pQ, [55], [IS]- It is expected that 
RB-McMBcr-SLAM would perform well in the presence of highly non-linear 
process and/or measurement models. 



7.4 Summary 

This chapter has detailed enhancements to the PHD-SLAM framework pro- 
posed in earlier chapters by exploiting alternative RFS map approximations 
and developing suitable filters which can be readily implemented via RB 
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techniques. Firstly, the RB-CPHD-SLAM filter was formulated, which re- 
laxes the Poisson distribution assumption on the number of features in the 
map, and propagates the map cardinality distribution as opposed to just its 
mean. Indeed, since the Poisson RFS is a special case of the IID cluster RFS 
(with a Poisson cardinality distribution), the RB-PHD-SLAM filter can be 
regarded as a special case of the RB-CPHD-SLAM filter. The formulation 
also included the weighting functions necessary for a RB implementation as 
well as suitable methods of state estimation. 

The RB-MeMBer-SLAM filter adopted a Multi-Bernoulli RFS map rep- 
resentation, which jointly encapsulates both the existence probability and 
the spatial density of each feature. In essence, it is conceptually equivalent 
to how the vast majority of existing vector-based SLAM approaches process 
uncertainty in the size of the feature map. However, since an RFS model 
is adopted, the tools of RFS filtering techniques are required to develop a 
suitable filter. The RB-McMBcr-SLAM filter directly approximates the joint 
multi-feature vehicle trajectory posterior as opposed to the moment approx- 
imation methods based on the PHD. 



Appendix A 

Concatenation of the Feature State m 
with Hypothesised Vehicle Trajectories 
Campbell's Theorem 



If L denotes the space of features and K denotes the space of vehicle states, 
Campbell's theorem |70| implies that the intensity of the point process on 
LxK formed by the Cartesian product of a point process on L, with intensity 
v, and a point process on the mark space (a vehicle pose particle) K, is 

v(Xk,m)=p(Xk\m)v(m), (A.l) 

where p(Xk\m) is the mark distribution given a point m of the original point 
process on L. Moreover, if the point process on L (the set of features) is 
Poisson, then the product point process on L x K is also Poisson [70]. As the 
RFS of the joint vehicle and map state is therefore Poisson, the derivation 
established in [5] can be incorporated into this work to include the joint 
vehicle-feature, augmented state. 



Appendix B 

Derivation of g h {Z k \Z&.k-\, X 0ik ) for the 
RB-PHD-SLAM Filter 



Recall equation 16.61 

,.. Iv s 9k{Zh\Mk,X k )Pk\k-i{M. k \Xo:k) 

gk\,£-k |A):k-l, A(|:fc J 

and the Poisson RFS approximations, 

II Vk\k-l(m\Xo :k ) 

Pk\k-i{Mk\X 0: k) ~ 



exp (J v k \k-i{rn\Xo:k)dm) 



]^[ Ufe(m|X :fc) 
Pt; (X fc |Xo: fc ) ~ 



exp (Jufe(m|Xo : fc)dm) 

B.0.1 The Empty Strategy 

Rearranging, and assigning M. k = gives, 

II t'fc|fc-l("l|^0:fc) 

,_ ,_ „ v ,«|,,y> v "'E-Mfc „ exp{j v k (m\X 0:k )dm) 

gk(Zh\Zo,k-i, -X-o-.k) = gk{Zk\y>,X k ) x 



II v k {m\X :k) cxp(f v ki k-i(m\X :k)dm) 

m£M h 

Since, M.k = 0, the empty set measurement likelihood is that of the clutter 
RFS (Poisson), 

n c k (z\x k ) 

g k (Zk\9,X k ) 



exp(J c k (z\X k )dz)' 



The PHDs ffc|fc-i and v k are empty, implying their product is 1, rh.fe|fc_ 1 
/wfc|fc-i("i|^0:fc)rfw and rtifc = / v k (m\X 0:k )d<m, giving, 
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g k (Zk\Zo: k -i,Xo:k) = ][ c fc (z|Xfe)exp f rhfe - m fc | fc _i - c k (z\X k )dz 

z£Z k ^ J J 

Note that while for the empty map choice, the likelihood g k (Z k \ZQ :k -i, 
Xo : k) does not contain a measurement likelihood term g k (Z k \A4 k , X k ), the 
history of measurements and trajectories are incorporated into the predicted 
and updated intensity terms, whose integrals appear as the terms ttlfeife_i and 
m k respectively. 

B.0.2 The Single Feature Strategy 

Assigning M. k — {m}, with m being chosen according to a given strategy, 

II v k\k-i(™\X .. k ) 

,,,, v s ri\- v\ "i£M k cxp(m fe ) 

g k (Z k \Z 0:k _ 1 ,X ..k) = g k (Z k \m,X k ) x — — — - — r— x r- 

11 v k (m\X 0:k ) exp(m fe | fe _i) 

m£M k 
fcr \- v \ v k\k-l{m\ X 0:k) ,„ s 

= g k (Z k \m,X k ) x -—— — — x exp (m fc -m fc | fc _ij 

v k (m\X 0:k ) 

If M k = {m}, thus from (|5^|) . 



(I - P D (rh\X k )) II c k (z\X k 
g k (Z k \rh,X k ) = — 



exp(/c fc (2:|A" fc )dz) 

J2zez k [ II c k (z\X k ))g(z\fh,X k ) 



P D (rh\X k )- 



exp(J c k (z\X k )dz) 
then, 

(1 - P D (rh\X k )) n c k (z\X k ) 

g k {Z k \Z ; k --i_,X ; k ) — — h 

V exp(J c fe (z|X fe )d2) 

E z « fc f I! c k (z\X k ))g(z\rh,X k ) 
PD(m\X k ) 



cxp(J c k (z\X k )dz) J u fc (m|X fc )exp(m fc | fe _i - m k ) 



Appendix C 

Fast SLAM Feature Management 



This appendix details the feature management routine developed for Fast- 
SLAM in a cluttered environment, providing it with knowledge of the de- 
tection and false alarm probabilities for a fair comparison with the RFS ap- 
proach. As with standard approaches Q] , tentative new features are declared 
for unassociated measurements. The 'existence probability' of each feature, 
Pg fc , given a 95% confidence gate and prior existence probability of Pgfk_ 1: 
then evolves through a binary Bayes filter according to 



Step 1: (Obtain association details within FoV) 

J = {j £ Mk\m? £ FoV & m-'not associated.} 
J = {j e M k \m j e FoV & m? associated.} 

Step 2: (Calculate hit, miss and association probabilities) 

FA = K/(RmAX X 7r ) 

P { r!L = (1 - Pd) x Pg-i + Pd x 0.05 x pg_ x 



p(J) _ p p(J) p(J) 

Piilc = ^|5 fc |- 1 / 2 exp(-0.5vS J 7 1 v T ) 
Step 3: (Update Existence probabilities) 



P 



p(J) 
(J) miss 



E * Pil + Ci-P^Ki-pg^) 



w 



p 



(./) Phit 



'P^ + PFAd-P^ 



This ad-hoc but effective routine enhances the robustness of standard 
SLAM feature management as shown in figures ROl and RTBl when exposed to 
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high clutter rates. Thus both the benchmark and proposed approach receive 
the same information for each filter loop. However, one of the fundamental 
merits of the proposed RFS framework is that feature management (and data 
association) are jointly incorporated into a single SLAM Bayesian update. 
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